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ABSTRACT 


Today’s  modern  avionics  systems  rely  heavily  on  the  integration  of  Global  Posi¬ 
tioning  System  (GPS)  data  and  the  air  vehicle’s  accelerations  obtained  by  an  Inertial 
Measurement  Unit  (IMU).  To  properly  resolve  the  GPS  and  the  IMU  data,  one  must 
have  an  understanding  of  the  different  coordinate  systems  involved.  Since  the  GPS 
provides  data  in  one  coordinate  reference  frame  and  the  IMU  measures  accelerations 
in  another,  transforming  the  data  freely  from  one  frame  to  the  next  is  imperative  if 
the  avionics  system  is  to  provide  meaningful  data  to  the  aircrew. 

This  thesis  provides  a  uniform  approach  to  analysis  and  design  of  an  inte¬ 
grated  GPS/IMU  avionics  system  using  MATLAB / Simulink  software  development 
tools.  Topics  covered  include: 

•  Coordinate  Systems  and  Transformations 

•  Fundamentals  of  Inertial  Sensors 

•  Tangent  Plane  Navigation 

•  Kinematic  Equations  and  Error  Analysis 

•  Global  Positioning  System  (GPS)  Sources  and  Errors 

•  Kalman  Filter  Design 

Emphasis  is  placed  on  addressing  the  theory  and  providing  detailed  examples 
to  support  each  topic. 
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I.  INTRODUCTION 


The  research  conducted  in  the  Avionics  lab  at  the  Naval  Postgraduate  School 
(NPS)  Aeronautical  Engineering  Department,  centers  around  the  flight  control  of 
Unmanned  Autonomous  Vehicles  (UAV’s).  Today’s  battlefield  has  become  increas¬ 
ingly  hostile  to  manned  airborne  vehicles  as  weapons  have  become  more  accurate 
and  lethal.  In  order  to  reduce  the  risk,  while  still  providing  forward  reconnaissance, 
the  military  has  increasingly  turned  toward  remotely  operated  aircraft.  Autonomous 
vehicles,  allow  the  commander  in  the  field  the  opportunity  to  survey  his  environment 
and  employ  the  UAV  as  a  targeting,  weapons,  or  intelligence  gathering  platform. 
The  reduced  risk  to  the  pilot  and  the  decreased  cost  when  compared  to  a  current  fleet 
aircraft,  makes  the  deployment  of  UAV’s  a  high  priority  in  future  battles. 

In  order  to  control  the  vehicle  in  a  variety  of  battlefield  environments,  precise 
knowledge  of  it’s  velocity  and  position  is  essential.  Using  on-board  inertial  navi¬ 
gation  systems,  the  vehicle’s  accelerations  can  be  precisely  measured  and  used  by 
on-board  navigational  computers  to  calculate  the  vehicle’s  velocity  and  position  in 
some  particular  reference  frame.  An  inertial  navigation  system,  alone,  can  provide 
accurate  position  information  in  the  short  term,  but  must  be  integrated  with  an  ad¬ 
ditional  source  if  precise  positional  data  is  to  be  maintained  for  the  long  term.  The 
UAV’s  requirement  to  operate  aboard  ships  or  in  rugged  locations  makes  it  essential 
that  extremely  accurate  position  information  is  calculated.  To  meet  this  requirement 
an  inertial  navigation  system  can  be  integrated  with  an  Global  Positioning  System 
(GPS)  receiver.  This  provides  the  vehicle’s  navigation  system  with  an  additional, 
highly  accurate  positioning  data  source. 
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The  purpose  of  this  thesis  is  to  provide  the  framework  for  designing  an  integrated 
inertial  measurement  /  GPS  system.  To  understand  the  system  integration  as  a  whole, 
it  is  necessary  to  have  a  solid  understanding  of  the  reference  frames  in  which  both 
systems  operate  and  how  the  angular  and  translational  motion  of  the  vehicle  relative 
to  these  reference  frames  is  calculated.  In  an  effort  to  make  the  three  dimensional 
(3-D)  relationships  and  the  associated  mathematics  more  understandable,  numerous 
3-D  graphical  examples  are  provided. 

A  brief  overview  of  current  inertial  sensors;  ring  laser  gyros  and  fiber  optic  gyros, 
is  included  in  order  to  understand  how  vehicle  accelerations  are  measured  and  how 
they  are  related  to  the  body  of  vehicle. 

As  with  any  system,  the  IMU  errors  are  inherent  in  the  IMU’s  computations. 
Detailed  analysis  of  errors  is  essential  if  the  IMU  accuracy  is  to  be  fully  realized  [Ref. 
1,  p.  231].  Since  the  IMU  is  not  a  position  fixing  system,  incorporating  a  GPS  as 
the  fixing  source  and  using  a  Kalman  filter  to  combine  the  two,  provides  the  best 
estimate  of  the  vehicle’s  velocity  and  position.  Kalman  filter  design  will  be  discussed 
and  developed  using  M.AT1LAB  /  Simulink  software  development  tools. 
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II.  COORDINATE  SYSTEMS 


To  develop  the  relationship  between  GPS  and  the  IMU,  an  understanding  of 
each  coordinate  system  is  essential.  In  this  chapter,  eight  coordinate  frames  will  be 
discussed  with  particular  attention  paid  to  only  four.  They  are  the  Earth-Centered- 
Inertial  (ECI)  coordinate  system,  the  Earth-Centered-Earth-Fixed  (ECEF)  coordi¬ 
nate  system,  the  tangent  plane  coordinate  system  and  the  Wander  Azimuth  coordi¬ 
nate  system. 

A.  TRUE  INERTIAL  {/} 

The  True  Inertial  coordinate  system  is  a  set  of  mutually  perpendicular  axes 
that  neither  accelerate  nor  rotate  with  respect  to  some  fixed  point  in  space.  Newton 
assumed  there  was  a  reference  frame  whose  absolute  motion  was  zero  (fixed  relative 
to  the  stars)  [Ref.  1,  p.  9],  and  it  is  in  this  reference  frame  where  they  are  valid. 
However,  Newton’s  laws  of  motion  can  also  be  applied  to  any  reference  frame  as  long 
as  the  proper  coordinate  transformations  are  applied. 

B.  EARTH-CENTERED-INERTIAL  {i} 

The  Earth-Centered-Inertial  (ECI)  coordinate  system  is  centered  about  the 
origin  of  the  earth  and  maintains  a  fixed  orientation  with  respect  to  some  inertial 
reference  in  space.  As  the  earth  rotates  the  frame  stays  oriented  with  respect  to 
this  inertial  reference.  Even  though  the  ECI  frame  is  referenced  to  the  earth  and 
translates  as  the  earth  rotates  about  the  sun,  it’s  rotational  motion  relative  to  the 
fixed  point  is  space  is  minimal  and  can  be  ignored.  This  allows  the  assumption  that 
Newton’s  laws  of  motion  apply  here  also.  In  the  cases  we  will  discuss,  we  will  assume 
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the  ECI  frame  stays  fixed  and  all  other  frames  will  rotate  with  respect  to  the  ECI 
frame. 

C.  EARTH-CENTERED-EARTH-FIXED  {e} 

The  Earth-Centered-Earth-Fixed  coordinate  system,  is  similar  to  the  ECI  sys¬ 
tem  except  that  the  frame  itself  is  connected  to  earth,  that  is,  it  rotates  with  the  earth. 
Every  twenty-four  hours  the  ECEF  frame  coincides  with  the  ECI  frame.  The  system 
origin,  as  the  name  implies,  is  centered  at  the  Earth’s  origin.  The  X-axis  is  directed 
through  the  intersection  of  the  Greenwich  meridian,  0°  longitude  and  the  equator,  0° 
latitude.  The  Y-axis  is  directed  from  the  origin  to  intersect  the  equator  at  90°  east 
longitude.  The  Z-axis  is  directed  along  the  Earth’s  spin  axis  from  the  origin  through 
the  north  pole  at  90°  north  latitude.  The  ECEF  frame  is  depicted  in  Figure  2.1.  The 


Figure  2.1:  Earth— Centered,  Earth— fixed  Coordinate  System 

earth-centered  earth-fixed  system  is  independent  of  the  mathematical  model  of  the 
earth’s  surface.  However,  both  the  geodetic  and  the  local  geodetic  systems  depend  on 
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the  specification  of  the  earth  model.  The  current  standard  for  modeling  the  surface  of 
the  earth  is  the  WGS-84  ellipsoid.  This  ellipsoid  is  generated  by  rotating  an  ellipse, 
whose  semi-major  axis  is  6378137.0  meters  and  whose  semi-minor  axis  is  6356752.3 
meters,  about  its  minor  axis.  The  resulting  closed  surface  is  the  model  of  the  earth’s 
surface.  The  true  north  pole  (conventional  terrestrial  pole)  and  true  south  pole  are 
the  endpoints  of  the  minor  axis  of  the  ellipsoid. 

D.  GEODITIC  COORDINATE  SYSTEM 

The  output  of  navigation  systems  used  on  aircraft  today  is  generally  latitude, 
longitude,  and  altitude  —  i.e.  resolved  in  the  geodetic  coordinate  system.  This  is  the 
system  used  for  describing  positions  of  most  earth  bound  objects.  Charts  developed 
for  long  range  land  and  sea  navigation  invariably  use  geodetic  coordinates. 

The  geodetic  coordinate  system  is  somewhat  analogous  to  spherical  coordinate 
system.  The  primary  difference  is  that  the  elevation  angle  or  latitude,  <f>,  is  the  angle 
between  the  ellipsoidal  normal  and  the  equatorial  plane.  This  means  that  the  ray 
that  defines  this  angle  does  not  intersect  the  equatorial  plane  at  the  exact  center  of 
the  earth.  Instead,  it  intersects  the  equatorial  plane  at  a  small  radius  outside  of  the 
center  as  shown  in  Figure  2.2. 

The  longitude,  A,  is  identical  to  the  spherical  concept  of  that  angle.  It  is  the 
angle  in  the  equatorial  plane  from  0°  latitude  and  0°  longitude  to  any  given  point. 
Finally,  h,  the  geodetic  height  or  altitude,  is  the  distance  along  the  ellipsoidal  normal 
away  from  the  surface  of  the  earth. 

E.  TANGENT  PLANE  COORDINATE  SYSTEM 

Typically,  pure  inertial  systems  navigate  in  a  so-called  tangent  plane  coordinate 
system,  before  outputting  position  in  geodetic  coordinates.  The  tangent  plane  system 
is  defined  by  passing  a  plane  at  any  point  on  the  earth’s  surface.  The  intersection  of 
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Figure  2.2:  Geodetic  Coordinate  System 

the  plane  with  the  surface  of  the  earth  becomes  the  origin  of  the  system.  The  x-axis 
points  toward  true  east.  The  y- axis  points  toward  true  north.  Lastly,  the  z-axis  is 
perpendicular  to  the  defining  plane  of  the  system,  away  from  the  center  of  the  earth. 
It  is  the  z  coordinate  of  the  triad  which  defines  a  point’s  altitude  in  this  system.  This 
frame  is  sometimes  referred  to  as  the  universal  {u}  frame.  This  is  shown  in  Figure  2.3. 

F.  NAVIGATION  COORDINATE  SYSTEM  {n} 

The  navigation  coordinate  system  is  attached  to  the  aircraft’s  frame.  It’s  ori¬ 
gin  is  located  at  the  center  of  the  aircraft’s  inertial  navigation  system  [Ref.  1].  It 
maintains  a  local-level  orientation  to  the  reference  ellipsoid  in  the  same  manner  as 
the  tangent  plane  but  the  orientation  of  the  axes  can  be  chosen  at  random.  If  the 
origin  of  the  aircraft  is  co-located  at  the  origin  of  the  tangent  plane,  then  the  tangent 
plane  and  the  navigation  plane  are  one  in  the  same.  The  frame  shown  in  Figure  2.3, 
represents  an  East-North-Up  ( ENU )  orientation  where  the  x-axis  points  east,  the 
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Figure  2.3:  Tangent  Plane  Coordinate  System 

y- axis  points  north  and  the  2-  axis  points  up.  The  orientation  of  the  frame  can  be 
specified  any  number  of  ways;  North-East-Up  ( NEU )  or  North-East-Down  (NED). 

G.  WANDER  AZIMUTH  COORDINATE  SYSTEM  {c} 

The  Wander  Azimuth  coordinate  system  is  a  frame  centered  at  some  reference 
point  on  the  body,  most  likely  the  center  of  the  aircraft’s  inertial  system,  the  origin 
of  the  navigation  frame.  The  x  and  y  plane  is  tangent  to  the  local  vertical  with  the 
2  axis  perpendicular.  The  frame  is  defined  with  respect  to  the  ECEF  frame  using 
longitude  A,  latitude  d>  and  the  wander  azimuth  angle  a.  The  angle  cx  is  positive  when 
the  frame  is  to  the  west  of  true  north,  the  angle  A  is  positive  east  of  the  Greenwich 
meridian,  and  <j>  is  positive  north  of  the  equatorial  plane.  If  the  wander  azimuth  frame 
is  aligned  with  the  ECEF  frame  at  the  Greenwich  meridian,  the  angle  a  is  zero.  The 
Wander  Azimuth,  co-located  tangent  and  navigation  planes  and  the  ECEF  frames 
are  depicted  in  Figure  2.4. 
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Figure  2.4:  Wander  Azimuth,  Navigation  and  ECEF  Coordinate  Systems 

H.  PLATFORM  FRAME  {p} 

The  platform  coordinate  system  is  the  right-handed  orthogonal  frame  defined  by 
the  input  axes  of  the  inertial  sensors.  If  the  inertial  navigation  device  is  a  “strapdown” 
model,  the  aircraft’s  accelerations  are  measured  in  the  platform  frame.  Imagine  the 
inertial  system,  perhaps  a  ring  laser  gyro,  bolted  to  the  frame  of  the  aircraft  and  all 
accelerations  experienced  by  the  aircraft  are  directly  measured  by  the  sensors  of  the 
inertial  unit.  Platform  accelerations  are  equivalent  to  body  fixed  accelerations  if  the 
unit  is  a  “strapdown  system”.  In  this  case  the  platform  frame  would  be  referred  to 
as  the  body  fixed  frame  {b}.  This  assumption  will  be  valid  throughout  this  thesis. 

I.  ACCELEROMETER  FRAME  {a} 

The  accelerometer  frame  is  a  non-orthogonal  frame  defined  by  the  sensitive  axes 
of  the  accelerometers  which  make  up  the  inertial  navigation  system  [Ref.  2].  If  the 
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axes  of  the  inertial  measurement  unit  are  not  properly  aligned  with  the  platform, 
errors  in  acceleration  measurements  can  exist  [Ref.  1], 
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III.  COORDINATE  TRANSFORMATIONS 


The  kinematic  equations  that  relate  the  motion  of  a  body  relative  to  a  fixed 
reference  frame  must  be  given  in  terms  of  the  relative  angular  velocity  and  coordinate 
transformations  between  each  frame.  In  order  to  use  these  coordinate  systems,  one 
must  be  able  to  transform  between  them  freely.  For  example,  satellite  positions  for 
the  GPS  computations  are  given  in  ECEF  coordinates.  Suppose  the  aircraft  posi¬ 
tion  is  wanted  in  the  tangent  plane  coordinate  system.  Therefore,  a  transformation 
between  ECEF  and  tangent  plane  systems  is  required.  For  the  three  systems  above, 
six  conversions  are  required.  By  being  able  to  convert  from  geodetic  to  ECEF,  and 
ECEF  to  local  geodetic,  one  can  convert  from  geodetic  to  local  geodetic  by  chaining 
the  two  conversions  together.  Thus,  the  only  four  transformations  discussed  are: 

•  geodetic  to  ECEF 

•  ECEF  to  geodetic 

•  ECEF  to  tangent  plane 

•  tangent  plane  to  ECEF 

A.  EULER  ANGLES 

Euler  angles  are  used  to  define  the  orientation  of  two  coordinate  systems  with 
respect  to  each  other.  For  example,  consider  the  aircraft  body  fixed  coordinate  system 
{6}  and  how  it  is  oriented  with  respect  to  the  tangent  plane  or  universal  frame  {u}. 
We  define  the  angles  the  aircraft’s  body  makes  with  this  system  in  terms  of  roll  <j>, 
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pitch  9,  and  yaw  xb.  Written  in  vector  format: 


'  <t>' 

A  =  9  (3.1) 

.  . 

Figure  3.1  represents  body  and  universal  coodinate  frames  where  the  origins  are 
collocated. 


z 


Figure  3.1:  Collocated  Universal  {u}  and  Body  {6}  Coordinate  Systems 

x 

Given  a  vector  y,  resolved  in  {u}:  yu  =  y  ,  what  are  the  components  of  y 

z 

in  {6}  or  y\P-  This  can  be  accomplished  using  Euler  angles  as  follows.  First,  let’s 
rotate  the  {u}  frame  about  the  z-axis.  Since  the  angle  ?/>  describes  rotation  about 
the  z  axis,  our  first  rotation  matrix  will  be  a  function  of  xj>.  Figure  3.2  depicts  the 
rotation  about  the  z-axis.  The  z-axis  should  be  considered  positive  outward  from  the 
x-y  plane.  The  rotation  is  considered  positive  using  the  right-hand-rule. 

The  new  axes  of  the  rotated  frame  have  been  labeled  (x\y',z').  Since  the 
transformation  matrix  describes  the  trigonometric  relationships  between  the  old  and 
new  frames  let’s  use  Figure  3.3  to  determine  what  these  relationships  are. 
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The  same  relationship  exists  for  y' .  In  this  case  the  a:  sin  xp  component  is  in  the 
direction  opposite  of  our  sign  convention.  Therefore  y'  written  as  a  function  of  the 
components  of  x  and  y  becomes: 


y'  =  —  x  sin  xp  +  y  cos  xp  (3-3) 

Since  the  rotation  occurred  about  the  2-axis,  z'  =  z.  Gathering  the  terms  in  matrix 
format  we  have  the  following  transformation  matrix: 

x'  cosxp  sim p  0  1  [  x 

y'  =  —  smxp  cosip  0  y  .  (3.4) 

_  z'  \  0  0  1  J  [  2 

Let’s  now  perform  the  rotation  about  the  new  y'  axis.  This  time  the  rotation 
angle  is  0  and  the  resulting  axes  will  be  ( x ",  y",  z").  Figure  3.4  shows  the  relationship 
between  the  new  axes. 


Figure  3.4:  Rotation  About  the  y'  Axis 


Using  the  same  process  we  used  to  calculate  (x',y',  z'),  we  develop  a  transfor¬ 
mation  matrix  based  on  the  trigonometric  relationships  between  the  new  axes.  These 
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relationships  are  shown  in  Figure  3.5. 


z' 

'**-  z'sinQ 

Figure  3.5:  Trigonometric  Relationships  Between  Rotated  Frames 

The  new  transformation  matrix  becomes: 

x"  cos#  0  —  sin#  x' 

y"  =  0  10  y’  .  (3.5) 

z"  _  sin0  0  cos 0  z' 

Notice  that  the  negative  sign  now  appears  in  the  first  row  with  the  sin  term. 
This  is  due  to  the  fact  that  the  rotation  was  through  the  angle  6  in  the  positive 
direction,  or  nose  up.  The  x"  component,  —z'  sin0,  was  in  the  direction  opposite  of 
the  sign  convention  chosen. 

The  last  transformation  will  be  about  the  x"  axis  which  is  a  rotation  of  <j>, 
right  wing  down.  This  is  the  final  orientation  of  the  body  frame  and  results  in  a 
transformation  matrix: 
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"  x'"  i  o  o  i  i  r  xn 

y'"  =  0  coscp  sind>  y" 

z'"  0  —  sin<^  cos (f>  z" 

Combining  equations  3.4,  3.5,  and  3.6,  we  have  the  final  (3-2-1)  transformation 

from  ( x,y,z )  to  (xb,yb,zb)  as: 

x  10  0  cos#  0  —  sin#  ]  cos^>  sin^>  0  x 

y  =  0  cos <j)  sind  0  10  —  sim#  cosip  0  y 

z  J .  [0  —  simp  coscp  sin#  0  cos#  0  0  1  2 

(3.7) 

When  performing  transformations,  the  Euler  angles  associated  with  each  rota¬ 
tion  are  not  vectors.  That  is,  #  +  <p  ^  <p  +  #.  This  implies  that  the  order  of  rotation 
is  important.  In  the  previous  example,  the  rotation  order  was  3-2-1,  or  ip,9,<p.  To 
verify,  perform  a  2-3-1,  or  ip ,  d>,  #  and  compare  the  final  transformation  matrix  with 
equation  3.7. 

B.  GEODITIC  TO  ECEF  COORDINATE  TRANSFORMATION 

In  order  to  compute  the  transformation  from  geodetic  to  ECEF  coordinates, 
three  auxiliary  quantities  —  /,  e,  and  N  —  must  first  be  defined  [Ref.  5].  The 
flattening  factor,  /,  represents  the  relative  flatness  of  the  ellipsoid.  A  zero  flattening 
factor  would  mean  an  unflattened  ellipse  (a  sphere),  while  a  unity  value  would  mean 
a  totally  flattened  ellipsoid  (a  circle  in  the  plane  perpendicular  to  the  minor  axis). 
The  mathematical  definition  of  /  is 

.  (3.8) 

a  ' 

where  a  and  b  are  the  semi-major  and  semi-minor  axes  of  the  ellipsoid,  respectively. 

Directly  related  to  the  flattening  factor  is  the  eccentricity,  e.  It  is  defined  by 

£2  =  2/  -  f.  (3.9) 
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The  eccentricity  is  a  variable  similar  to  the  flattening  factor.  It  represents  how  close 
the  ellipsoid  is  to  a  sphere.  It,  too,  is  zero  for  a  sphere  and  one  for  a  completely 
flat  figure.  The  eccentricity,  rather  than  the  flattening  factor,  is  typically  used  in 
coordinate  transformations. 

Lastly,  N  is  the  length  of  the  ellipsoidal  normal  from  the  ellipsoidal  surface  to 
its  intersection  with  the  ECEF  z- axis.  Mathematically,  N  is 


where  (j>  is  the  geodetic  latitude. 

Using  these  quantities,  one  may  define  the  transformation 


(3.10) 


x  =  (N  +  h)  coscj)  cosA 
y  =  (N  +  h)  cos (j>  sinA 

2  =  [N(l  -  £2)  +  h]sm(j).  (3.11) 


C.  ECEF  TO  GEODETIC  COORDINATE  TRANSFORMATION 

The  transformation  from  ECEF  to  geodetic  coordinates  is  clearly  the  inverse  of 
the  process  presented  in  the  previous  section.  First,  A,  the  longitude,  can  be  found 
by  dividing  first  two  expressions  of  Equations  3.11  yielding 


tan  A  = 


(3.12) 


By  examining  the  geometry  in  Figure  2.2,  one  can  determine  the  following  relationship 


tan  (j)  = 


(. N  +  h )  sin^» 

aA2  +  y2 


(3.13) 


which  is  a  non-linear  equation  in  <f>.  Solving  the  third  of  Equations  3.11  for  (N  + 
fi)sin  <t>  and  substituting  into  Equation  3.13  yields 


tan  <b  = 


z  ,  £2iVshA 

^a:2  +  j/2  z  ’ 


(3.14) 
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which  is  still  an  analytically  unsolvable  equation  in  geodetic  latitude.  To  solve  this 
equation,  one  initially  assumes  that  h  is  zero,  an  excellent  assumption  for  intra- 
atmospheric  flight.  Now,  the  z  equation  in  Equation  3.11  can  be  simplified  to 

Zh= o  =  iV(l  -  e2)  sin <f>.  (3.15) 


This  equation  can  be  substituted  into  Equation  3.14  to  give  the  initial  solution  for  4> 


tan  (j) j 


1  —  e2  \fx 2  +  y2 


(3.16) 


This  initial  solution  for  <t>  can  be  substituted  back  into  the  second  term  of  Equa¬ 
tion  3.14  to  yield  an  updated  <j>.  Iteration  of  this  process  commencing  with  Equa¬ 
tion  3.14  continues  until  the  geodetic  latitude  stops  changing.  Finally,  solving  Equa¬ 


tion  3.13  for  h 


which  completes  the  conversion. 


4c2  +  y2 
cos  6 


(3.17) 


If  we  assume  a  spherical  earth  model,  a  and  b  in  Equation  3.8  are  equal,  resulting 
in  a  flattening  factor  /  =  0.  (Assume  a  =  b  =  R0,  radius  of  the  earth).  Substituting 
/  in  Equation  3.9,  the  eccentricity  factor  e  also  equals  zero.  Applying  these  values 
to  Equation  3.10,  the  length  of  the  ellipsoidal  normal  N  is  now  simply  the  radius  of 
the  earth  R0.  Substituting  these  values  in  equations  3.11  through  3.18,  the  altitude 


h  can  now  be  defined  as 


h=V^ 

cos  q> 


(3.18) 


The  physical  representation  can  be  seen  in  Figure  3.6. 


D.  ECEF  TO  TANGENT  PLANE  COORDINATE  TRANSFORMATION 

Anytime  one  uses  a  local  geodetic  or  tangent  plane  coordinate  system,  one  must 
first  specify  the  geodetic  coordinates  —  latitude  and  longitude  -  of  the  origin.  Once 
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Figure  3.6:  ECEF  Spherical  Model  with  Altitude 


specified,  the  origin  must  be  expressed  in  ECEF  coordinates.  Then,  a  vector  from  this 


origin  to  the  point  being  transformed  can  be  formed,  resolved  in  the  ECEF  system 


Ax 

X2  —  Xi 

Ay 

= 

2/2  2/i 

.  Az  . 

e 

.  Z2  -  zx 

where  the  ‘2’  subscript  denotes  the  point  being  transformed. 


(3.19) 


The  product  of  two 


rotation  matrices  operates  on  the  difference  vector  defined  in  Equation  3.19  to  yield 


the  local  geodetic  coordinates  of  the  point  Pu,  where  the  subscript  {u}  represents  the 


universal  frame. 


X 

—  sinA 

cosA 

0  ‘ 

Ax 

Pu  = 

y 

= 

—  sin^>  cosA  —  sin<^  sinA 

COS(f> 

Ay 

z 

u 

cos <j>  cosA 

cos <f>  sinA 

sin  <f> 

.  . 

where  A  is  the  geodetic  longitude  and  <j>  is  the  geodetic  latitude. 


(3.20) 


E.  TANGENT  PLANE  TO  ECEF  COORDINATE  TRANSFORMATION 

The  transformation  from  tangent  plane  to  ECEF  can  be  derived  by  merely 
reversing  the  process  developed  in  the  previous  section. 
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First,  the  origin  of  the  local  geodetic  system  must  be  converted  from  geodetic 
to  ECEF  coordinates.  Next,  the  inverse  of  the  rotations  performed  in  Equation  3.20 
must  be  executed  yielding  the  A  vector  from  the  origin  of  the  tangent  plane  system 
to  the  point  being  transformed.  This  vector,  expressed  in  ECEF  coordinates  is 


Ax 

—  sinA 

—  sin<^>  cosA 

cos</>  cos  A 

X 

Ay 

— 

cos  A 

—  sin (f>  sinA 

cos  (j>  sinA 

y 

.  Az  . 

e 

0 

cos<b 

sin^> 

z 

u 

(3.21) 


Clearly,  adding  the  A  vector  to  the  position  of  the  origin  of  the  tangent  plane  system 


(both  now  in  ECEF  coordinates)  completes  the  transformation: 


X 

i 

£ 

o 

Ax 

y 

= 

yorig 

+ 

Ay 

z 

e 

i 

£ 

o 

e 

_  A  z  _ 

F.  VECTOR  NOMENCLATURE 


(3.22) 


In  order  to  proceed  further,  there  must  be  a  firm  understanding  of  the  nomen¬ 
clature  used  to  define  the  vector  of  interest  and  the  frame  of  reference  where  it  is 
resolved.  The  vectors  we  will  be  interested  in  are  position  P,  linear  velocity  V,  and 
angular  velocity  u.  Each  vector  has  a  magnitude  and  direction  and  a  reference  frame 
to  which  it  is  attached.  For  the  purpose  of  this  discussion,  we  will  be  concerned  only 
with  two  reference  frames,  the  tangent  (universal),  denoted  by  u,  and  the  body  fixed 
frame,  denoted  by  {&}.  As  we  become  more  familiar  with  other  reference  frames  we 
will  use  different  characters. 

Let  Q  be  an  arbitrary  point  located  on  the  body  of  the  aircraft.  We  can  draw  a 
position  vector  to  that  point  from  the  origin  of  the  body  reference  frame.  This  vector 
would  be  identified  as  bPQ .  The  P  stands  for  a  position  vector,  the  subscript  Q  for  the 
point  of  interest  and  the  superscript  b  for  the  reference  frame  the  point  is  measured 
with  respect  to.  Simply  put,  bPQ  is  the  position  of  point  Q  resolved  in  { b }.  As  with 
any  vector,  the  position  vector  can  be  described  in  terms  of  any  reference  frame.  An 
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example  would  be  to  define  the  point  Q  with  respect  to  the  universal  frame  or  uPq. 
Figure  3.7  illustrates  the  position  vectors  in  both  reference  frames. 


Figure  3.7:  Position  Vectors  of  Point  Q 

If  the  position  vector  P  refers  to  the  origin  of  a  reference  frame,  the  subscript 
will  be  ox  where  o  refers  to  the  origin  and  x  refers  to  the  frame  of  interest.  Therefore, 
‘Pob  would  refer  to  the  position  of  the  origin  of  the  {b}  frame,  resolved  in  the  universal 
frame  {u}. 
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G.  TRANSFORMATION  OF  ANGULAR  VELOCITIES 


Now  that  we  can  describe  the  position  of  a  point  on  a  body  with  respect  to  an 
arbitrary  reference  frame,  we  also  need  to  express  the  motion  of  the  body  as  well. 
As  the  vehicle  moves  in  inertial  space,  the  angles  that  describe  it’s  relative  position 
change  as  well.  Question:  How  do  we  define  the  Euler  angular  rates  in  terms  of  the 
body’s  angular  rates? 

'  <t>  ' 

A  =  9  =  ?w  =? 

.  P  . 

Answer:  Let  f \n  be  the  angular  velocity  of  {6}  with  respect  to  {n}.  We  can 
obtain  A  by  considering  the  derivative  of  the  Euler  angle  in  the  coordinate  system  it 
was  defined.  The  total  angular  velocity  is: 


0  0  <j p 

£lbn =  C  0  +  DB  9  +  D  0  (3.23) 

.  ip  J  0  J  0  _ 

Where  C  is  the  transformation  matrix  from  {n}  — *•  {b}: 

cosip  sin  ip  0 
-  simp  cos  ip  0 
0  0  1 

DB  is  the  transformation  matrix  from  {n'}  — >  {&},  and  D  is  the  transformation 
matrix  from  {n"}  — s ►  {6}.  Combining  terms  results  in: 


10  0  cos 0  0  —  sin# 

C  =  0  cosp  skip  0  10 

0  —  sinp  cosp  sin^  0  cos^ 

' - V - /V - V - 

D  B 


■  1 

0 

—  sin^ 

\P- 

10  = 

0 

cosp 

cos  9  sin^> 

9 

0 

—  sinp 

cos 9 cosp 

L  P . 

(3.24) 


Finally,  1C  —  lCS(co),  where: 

0  —  loz  UJy 

S(u j)  =  ujz  0  -ux  (3.25) 

_  —  Uy  UX  0 

is  a  skew  symmetric  matrix. 
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H.  SPECIFIC  TRANSFORMATIONS 
1.  ECI  to  ECEF:  fC 

First,  we  need  to  determine  Earth’s  sidereal  rate,  The  subscript  ei  im¬ 
plies  the  angular  rate  of  {e}  measured  with  respect  to  {z}.  This  convention  is  the 
reverse  of  the  nomenclature  in  [Ref.  1]  and  will  be  maintained  due  to  it’s  simplic¬ 
ity.  Assume  the  navigation  frame  and  universal  frame  are  co-located  as  shown  in 
Figure  3.8.  Consider  Figure  3.9.  It  is  clear  from  this  figure  that  f 2et-  resolved  in  {n} 


Zi,  Ze 


Figure  3.8:  ENU  Navigation  Frame 


is: 


"0*  = 


0 

flcos^ 

flsin^ 


where  ft  is  the  magnitude  of  Earth’s  rotation  and  is  given  by: 

360° 


n  = 


23  + 


56+ 3 


60 


n  =  15.041^  =  7.2  xio-5  — 

hr  sec 


(3.26) 
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Figure  3.9:  Earth’s  Sidereal  Rate 

where  one  sidereal  day  equals  23hr  56min  4.09sec. 

Resolving  the  angular  velocity  in  the  ECEF  {e}  frame: 

'  0  ' 

enei  =  0  (3.27) 

n 

This  vector  implies  that  the  only  component  of  Oe;,  in  the  {e}  frame  is  in  the  2 
direction,  see  Figure  3.8. 

Now,  consider  the  rotation  viewed  in  the  equatorial  plane,  where  zt-  =  ze. 
Over  a  time  At,  the  earth  rotates  at  a  rate  flA f,  figure  3.10.  The  transformation 
matrix  from  {i}  to  {e}  can  be  calculated  as: 

x  I"  cos f)At  sinflAt  0  1  [  x 

y  —  —  sinflAt  cosflAt  0  y  (3.28) 

z  0  0  1  z  . 

LJeL  JLJ? 

Let  A  =  —  —  where  n  is  chosen  such  that  0  <  A  <  2-k  &  [t  — 10)  is  the  time 

elapsed  since  vernal  equinox.  Since  the  two  frames  are  orthogonal  the  transformation 
from  {e}  to  {i}  would  be  the  inverse  of  the  transformation  matrix  in  Equation  3.28. 
Substituting  A  and  inverting  the  matrix  gives: 
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Figure  3.10:  Sidereal  Rate  in  the  Equatorial  Plane 


X 

’  cA  —  sA  0  " 

X 

y 

— 

sA  cA  0 

y 

(3.29) 

z 

'  i 

l 

r-H 

O 

O 

z 

e 

Finally,  determine  a  point  on  the  Earth’s  surface  in  terms  of  latitude  and  longitude 
(<?>,  A).  Let  P  be  the  vector  describing  the  point  Q ,  Figure  3.11,  then: 


Figure  3.11:  Point  on  Earth’s  Surface 
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(3.30) 


eP  = 


where  R  is  the  radius  of  the  Earth. 


R  cos(j>cos  A 
Rcos<f>sm\ 
Rsin<f> 


2.  ECEF  to  Navigation: 

Now  consider  the  transformation  from  {e}  to  {n}.  Let  the  orientation  of 
the  navigation  frame  be: 


Figure  3.12:  ECEF  to  Navigation 


Then 

•  Rotating  around  ze  by  A  (longitude) 

•  Rotating  around  ye  by  <f>  (latitude) 
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to  obtain: 


3 

II 

X 

y 

— 

z 

n  L 

c<p 

0 

—sc i 


0  scf> 

1  0 

0  ccj) 


cX  sX  0 
-sX  cX  0 
0  0  1 


X 

y 

z 

=  JC'-fij 


(3.31) 


Note:  Since  the  origins  in  Figure  3.12  are  not  coincident  =>  Eq.  3.31  is  not  completely 
correct.  We  need  to  include  position  of  the  origin  of  the  navigation  frame,  eP0n. 


'Pq  =  ea,  +  t«  =  jc(*p0  -  ep0n) 


Use  vector  calculus  to  define  unit  vectors  for  {n}: 


IE  = 


xR\ 


z  _  -Hx (net  xR) 

~  \Rx(QeixR)\ 


*U  =  7Zi 


K 

\R\ 


3.  ECEF  to  Wander  Azimuth  {c}: 

To  obtain  S C  we  must  rotate  {n}  by  an  angle  a  around  xn : 
where: 


c 

n 


c  = 


10  O' 
0  ca  $a 
0  —sa  ca 


Then  ^7  =  £(7  £C,  see  figure  3.13. 


(3.32) 


Now,  for  {n}  =  NEU 


IC  = 


ca  —sa  0 
so:  ca  0 
0  0  1 


(3.33) 
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Here: 


zn=  v 

Figure  3.14:  Wander  Azimuth  (NEU) 


For  {n}  =  UEN 


Figure  3.15:  Wander  Azimuth  (UEN) 

Look  carefully  at  the  orientation  of  {c}  and  {n}.  To  understand  each  element  of  the 
transformation  matrix,  write  out  each  component  of  {c}  separately  and  compare  with 
the  matrix  form  in  Equation  3.34. 

xc  =  —  yn  sin  a  +  zn  cos  a 
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Vc  =  Vn  cos  a  +  zn  sin  a 


X 

0  —sa  ca. 

X 

y 

= : 

0  eo:  sa 

y 

z 

c 

. 1  o  o 

z 

4.  Navigation  to  Body:  °nC 

Define  {b}  &  {n}  using  right  hand  rule  =>■  NED 


(3.34) 


Figure  3.16:  NED  Orientation 

Convention: 

•  4>  positive  when  right  wing  down. 

•  $  positive  when  nose  up. 

•  tl>  positive  when  nose  rotates  N  — >  E. 

Then: 

hnC  =  C^CeC^ 
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X 


Figure  3.17:  Positive  <p,  Positive  6 ,  Positive  ip 

where: 

10  0  c6  0  —sO  cip  sip  0 

C+  =  0  ccp  s<p  ,  Ce  =  0  1  0  ,  C,p  =  -sip  op  0  (3.35) 

_  0  —s<p  c<p  _  sO  0  cO  0  0  1 

I.  COORDINATE  FRAMES  FOR  INS  MECHANIZATIONS 

Types  of  INS  configurations 

•  Local  level  (torqued) 

•  Space  Stabilized 

•  Strap  down 


Local  Level 

The  x,  y  accelerometers  are  always  in  a  plane  tangent  to  local  ellipsoid  (ideally 
{n}).  Torque  is  applied  to  vertical  axis  to  maintain  certain  orientation  of  x,  y  axes: 
There  are  four  types  of  local-level  mechanizations: 
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Figure  3.18:  Torque  Applied  to  Maintain  Orientation 

•  North  Slaved 

•  Unipolar 

•  Free  Azimuth 

•  Wander  Azimuth 

In  order  to  maintain  a  local-level  orientation,  the  accelerometer  platform  must  be 
torqued  to  align  with  the  axes  of  the  local-level  frame.  Each  of  these  systems  is 
defined  by  the  azimuth  torquing  rate. 

Question:  How  to  compute  the  torque?  • 

Answer:  Must  know  angular  velocity  of  platform  with  respect  to  the  inertial  frame. 
Let:  {n}  =  ENU 
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Figure  3.19:  Geographic  and  Inertial  Torque  Model 


Now, 

—  ^ne  “f"  flei  i  where  Qni  —  angular  velocity  of  (  n  1  'tvvt  ( z )  (  3.36 ) 

Note:  in  Siouris  [Ref.  1],  p  is  used  to  define  0ne. 

Recall  Equation  3.26 


— 


0 

flcos<^> 

_  rising 


(3.37) 


Let: 


be  the  velocity  of  {n}  wrt  {e}. 


v  = 


ve 

Vn 

Vu 


(3.38) 


Now,  the  angular  velocity  f 1ne  can  be  described  as  a  function  of  the  distance  from  the 
Earth’s  center  and  the  linear  velocity  v  at  a  point.  Therefore,  let: 


^ne 


loe 

Ujj 

UN 


(3.39) 
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Consider  Figure  3.20,  the  angular  velocity  u>e  can  be  expressed  in  terms  of  the 


radial  distance  and  the  linear  velocity  u/v,  or: 


E 


Next,  from  Figures  3.21  and  3.22  u>n  and  ujj  are: 


N  vE 


Figure  3.21:  w# 


VE 


(3.40) 


(3.41) 


(3.42) 

(3.43) 
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where 


and 


ei 


Figure  3.22:  UJJJ 


£  =  1 - -  =  eccentricity  factor 

a1 


a  =  semi-major  axis  and  b  =  semi-minor  axis 


(3.44) 


From  Figure  3.23  we  obtain 

'  .  -4> 

eOm-  =  (A  +  ft)  cos <j> 

(A  +  Cl)  sin</> 

Let  the  orientation  of  the  navigation  frame  {n}  be  UEN.  Therefore,  Qni  re¬ 
solved  in  the  {n}  frame  is: 

c<j)  0  s<j>  cA  sX  0  "  0  1  a  0  s<j>  1  I"  0 

Qni =  0  10  — sA  cA  0  0  0  1  0  — (j) 

.  -s<j>  0  C(f>  \  [  0  0  lJLA  +  fl_  _  -S(f>  0  c(f>  o 


(3.46) 


(A  +  f 1)s<f>  " 

0  (A  -f- 

nQni  =  0  + 

—  (f)  =  —<b 

(3.47) 

(At  r V)c<f> 

0  (A  +  0,)ccj> 

The  negative  sign  in  front  of  the  term  comes  from  the  orientation  of  the  axes.  Since 
the  y- axis  is  east,  the  upward  rotation  of  <f>  is  negative  using  the  right-hand-rule. 


(3.45) 
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Figure  3.23:  £lni 


To  verify,  point  the  thumb  in  the  direction  of  the  y- axis  and  curl  the  right  hand.  A 
positive  rotation  would  be  down  instead  of  up.  Hence,  the  negative  sign. 

Now  convert  to  ENU  orientation 

-4>  ' 

(A  +  Q)c4>  (3.48) 

(A  +  f V)s(f) . 

J.  PLATFORM  MECHANIZATIONS 

Let  a  determine  the  drift  (wander)  angle  of  {c}  =  {p}  with  respect  to  {n}  =  {p}, 
the  geographic  frame  and  is  a  function  of  both  the  sidereal  rate,  Clei  and  fine,  as  shown 
in  Equation  3.49. 

0  ca.  sol  0  ]  I"  0 

0  =  —  sa  ca  0  0  (3.49) 

.  a  J  0  0  1  J  [  (A  +  n)s(j)  _ 

Figure  3.24  shows  the  commanded  drift  angle  a.  The  rate  of  change  of  a  or  a  is 
the  difference  between  the  platform  angular  velocity  about  the  2-axis  cozp  and  the 
geographic  angular  velocity  u>zg  or 
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Figure  3.24:  Platform  Frame  {p}  with  Angular  Rate,  a 

In  order  to  keep  the  platform  aligned  we  must  command  the  angle  a  at  a  rate  equal 
to  uzp,  we  which  we  have  defined  as  lozc.  We  can  now  define  each  mechanization  in 
terms  of  a. 

North  Slaved 

To  maintain  a  north  pointing  system  we  must  keep  xp  pointing  North,  {NEU}  ori¬ 
entation.  If  we  apply  torque  to  keep  a  =  0 

=>  iozc  =  (A  +  0 )scf> 

Free-Azimuth 

In  a  Free-Azimuth  system,  the  vertical  axis  is  not  torqued,  therefore: 

uzc  =  0 

Substituting  into  Equation  3.50,  we  get 

6l  =  —  uzg  =  — flej  sin  <j> 

The  equation  at  the  top  of  page  46  in  [Ref.  1]  is  incorrect  and  should  be  the  same  as 
Equation  3.50. 
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Unipolar 


In  the  Unipolar  system,  the  wander  angle,  a,  is  kept  equal  to  the  longitude,  A,  of  the 
geographic  frame  or  a  =  ±A.  Substituting  into  Equation  3.50: 

lozc  =  LOzg  ±  A  (sign  changes  when  crossing  the  equator) 

Wander-Azimuth 

In  the  Wander-Azimuth  system,  commanded  torque  u>zc  is  equal  to  the  vertical  com¬ 
ponent  of  the  earth’s  sidereal  rate,  sin  <f>.  Substituting  into  Equation  3.49: 

a  =  f lei  sin  (j)  —  Qe{  sin  <f>  —  A  sin  <b 
a  =  A  sin  (j) 

(No  singularity  at  the  pole) 

Space  Stabilized 

Maintains  constant  orientation  wrt  inertial  space. 

Strap- down 

Inertial  sensors  are  mounted  directly  on  the  vehicle  ==-  transformation  from  sensor  to 
inertial  axis  is  computed  rather  than  mechanized. 


K.  PLATFORM  MISALIGNMENT 

Platform  to  Accelerometer: 

Imperfections  in  accelerometer  installation  result  in  errors  between  {p}  and  {a}. 
Assume  misalignment  angles  are  small:  i.e.  $<f>  «  <f>,  c<j>  «  1.  Error  angles: 

•  rotate  by  <f>  about  xv 

•  rotate  by  9  about  yp 
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•  rotate  by  ip  about  zp 


(3.51) 

(3.52) 

(3.53) 

(3.54) 

Ignoring  higher  order  terms,  we  get: 


1 

-9  1 

0 

iP 

-9  ' 

;c=  -ip 

l 

4>  —  I 3x3  + 

-ip 

0 

<t> 

9 

-<p 

1 

J 

9 

-<P 

0 

'<T 

=  hx3  +  S(  A),A  =  0  (3.56) 

.  ^  . 

pC  is  a  linear  function  of  A.  In  general,  for  small  changes,  Euler  angles 
act  like  vectors.  These  small  misalignment  angles  can  be  used  to  compensate  for 
misalignment  errors.  (In  this  derivation  we  have  assumed  {a}  to  be  orthogonal). 

L.  ESTIMATE  OF  PLATFORM  TO  NAVIGATION  TRANSFORMA¬ 
TION:  *C 

Let  be  the  estimate: 


C  +  8  (small  error) 

(3.57) 

^  (/  +  «JCSC)  f 

(3.58) 
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cip  sip  0  "  *  c9  0  —sO  i  r  0  O' 
—  sip  cip  0  0  1  0  0  ccp  s<p 

0  0  1  s0  0  c9  0  —  s<p  c<p 

'  i  ip  o  l  r  i  o  -9 1  r  i  o  o' 

=  i  o  010  01  <p 

0  0  1  J  [  0  0  1  J  [  0  -<p  1 

i  ip  o  ]  r  i  9<p  -9  * 

=  —ip  1  0  0  1  <p 

0  0  1  J  L  9  ~<t>  1 

1  9<p  +  ip  —0  +  ip<p 

—ip  1  <p  +  ipcp 

9  -<p  1 


Since  ’^5T  =  I  we  obtain: 

(/  +  (sp)  sc)  ;c  ic  (i  +;c(«jc)T)  =  i 

i  +  (<jcj  ;c  +  ;c(<srt7)T  +  (s’p^s’ff  =  /  (3.59) 

Since  8C8CT  ~  0,  we  get: 

=>(5p"C)JC=-[(^"C)^']T:=M  (3.60) 

Post  multiplying  the  left  and  right  sides  of  Equation  3.60  by  gives: 

8;C  =  MpnC  (3.61) 

Adding  to  both  sides  and  remembering  that  vnC  =  —  ”C  for  a  skew  symmetric 
matrix,  Equation  3.61  becomes: 

^npC  =  (I-M);C  (3.62) 
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IV.  INERTIAL  SENSORS 


A.  LASER  GYRO  FUNDAMENTALS:  PASSIVE  SAGNAC  INTERFER¬ 
OMETER 


Light  is  split  at  A  to  travel  cw  and  ccw.  Suppose  Cl  =  0.  Then: 

2t cR 
c 

Suppose  Cl  ^  0.  Let  X  be  the  distance  A  traveled  in  inertial  space  for  light  to  return 
to  beam-splitter.  Then: 


Then: 


t±  —  2i rR  ±Xfc,  whereA-t  —  RClt± 


,  27 rR  RClt± 

t±  = - + - -  =►  t± 

c  c 


2t eft 
cqp  RCl 
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Then: 


A t  =  4  -  i_  = 
—  27ri? 


2-kR 

c-m 
2RQ 


2ttR 
c  +  RCl 


L c2  -  i?2Q2J 

47r^2n 


c2  -  R2fl2 

4VRRC1  m 


1 

AtvR  Rfl 


(Z—ly 
c 


Rtt 

~RQ' 

2 

1  + - + 

+  ... 

c 

.  c  . 

Now,  to  a  first  approximation: 


47rfi/?2 

At  = - - —  -4=  Sagnac  effect 


The  optical  path  difference: 

A L  =  cAt  =  c 


47rO/?2 


47nR2  4t10 


A  =  AttR2  <=  area  enclosed  by  the  circular  path 
Sagnac  effect  is  used  in  RLG  and  FOG. 


B.  FIBER  OPTIC  GYRO 

0  is  measured  by  analyzing  the  phase  shift  caused  by  Sagnac  effect 

•  Ideal  behavior  assumes  reciprocity  the  paths  of  cw  and  ccw  beams  are  iden¬ 
tical  =>•  the  phase  shift  is  due  to  inertial  distance  traveled 

•  Non-reciprocity  may  arise  from  non-linear  index  changes  caused  by  unequal 
intensities  =>■  is  handled  by  using  broadband  source 

•  Non-reciprocity  due  to  external  magnetic  fields  like  Faraday  effect  can  be  re¬ 
duced  by  magnetic  shielding 
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Photo 

Detector 


Figure  4.2:  Fiber  Optic  Gyro  [Ref.  1,  p.  119] 


•  Fiber  length:  50m  to  1km 
FOG  elements: 

•  Laser  diode  that  acts  as  a  light  source 

•  beam  splitter 

•  coil  of  optical  fiber 


•  photo  detector 


Light  from  laser  diode  is  split  into  two  beams  =>  cw  8z  ccw.  The  beams  are  super¬ 
imposed  and  the  resulting  interference  is  monitored  =$>  measure  phase  shift  by  using 
photo  detector  =>  convert  light  into  electrical  current.  Current  is  sampled  every 
frame.  Let  A  be  the  wavelength  of  the  light.  Then: 


T  = 

and  phase  shift  A  d>  = 


A 


7 


c 

2 


T 

47rf IR2  c 

[27r27r(2  ir  R)  RSI] 


AttLR 

Ac 


Ac 

n,  for  iV  =  1,  number  of  turns 


For  N  >  1,  L  =  2irRN  and  A  =  t R2 


A  4>  = 


I<  = 


8tt NA 

Ac 

SttNA 

Ac 


0 


sensor  scale  factor 


Note,  Sagnac  effect  can  be  viewed  in  terms  of  At  or  A <j>. 
=4-  Two  practical  approaches  to  obtain  0: 


1.  Measure  A cf>  (=^  At) 

2.  Change  the  frequency  of  one  of  the  beams  until  A<jf>  =  0. 


Then: 


(4.1) 
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V.  TANGENT  PLANE  NAVIGATION 


A.  TANGENT  PLANE  EQUATIONS 

Consider  the  case  of  flight  in  a  limited  region.  Use  tangent  plane: 

X  =  North 
Y  =  East 

Z  =  Up:  Away  from  the  earth’s  center 


Figure  5.1:  Tangent  Plane 

Let  iji  be  the  unit  vector  in  the  direction  away  from  the  earth’s  center.  Then 
the  local  gravity  vector  g  acting  on  the  observer  is 


where 


R  =  P0  —  R0  and  R  = 

R0  =  vector  connecting  E.C.  with  the  origin  of  the  tangent  plane. 
P0  =  position  of  the  observer  in  the  tangent  plane. 

Let  P0  =  (a;,  y,  z)  then, 

PH  -  ^2  +  y2  +  (^  +  i?0)2 

X 

y 

z  +  R0 


I  GMjz±Ro} 

L  pii3  J 

Note,  for  flights  near  the  Earth’s  surface,  R  ~  R0,  then: 

9x 
<  9y 
.  9z 

Where 


B.  GENERAL  NAVIGATION  EQUATIONS 

•  Idea:  Compute  vehicle’s  position  based  on  accelerometer  readout  &  equations 
derived  above. 

•  Constraint:  Einstein’s  Uncertainty  Principle  =>•  Accelerometers  cannot  distin¬ 
guish  between  inertial  and  gravitational  forces. 


-■r-x 

JTLo 

-R-ay 

~9o  +  2^2 


9o  — 


GM 

m 


(5.1) 


R  = 


X 

0 

y 

— 

0 

z 

- , 

o 

a? 

I 

IR  — 


IWI 


,  a 

L  Pll 


Therefore  ,  the  local  gravity  vector  is: 


9  = 


GM 


II£13 

QM  y 

ii_r.ii3  y 
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Therefore:  Let 


Then 


Where 


R  —  geocentric  position  vector  of  vehicle 
XV  =  vehicle’s  inertial  velocity 

f  R  =  'V 

[  lV  =  A  +  gm(R )  •£=  Einstein’s  principle 


A  =  specific  force  measured  by  accelerations 
gm{R )  =  gravitational  acceleration 


(5.2) 


Equation  5.2  represents  a  system  of  differential  equations  that  can  be  integrated  to 
get  vehicle’s  position  in  an  inertial  frame. 

Recall,  r  —  A  +  g,  and  we  get: 

{Ax  —  x  —  gx  =  x  + 

Ay  =  y-gv  =  y  +  jfoy  ,  (5.3) 

Az  =  z  —  gzfaz  —  2  j^z  +  g0 

where 

GMR? -GM{z  +  Ro)ZR22-^A 

g°  ^  ™ 


gM[z  +  R0) 
R3 


(  d 

gM(z  +  R0) 

\9o  +  dz 

R 3 

[GMR3  -  (GM(z  +  R0)3R2)  (^±P)] 
_ 

2  GMRRo  \ 

9°  — w~z ) 


The  mechanization  for  Ax,  Ay,Az  is  shown  in  Figure  5.2. 
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X(O)  x(0) 


Figure  5.2:  Block  Diagram  of  Inertial  Navigation  System 

Solve  the  differential  equations  in  Equation  5.3  for  and  to  get: 


Therefore,  for  constant  thrust  acceleration  in  x  and  y  direction  the  vehicle  is 
displaced  to  an  average  position  such  that  the  gravitational  component  in  x  or  y 
direction  is  equal  to  imposed  thrust  acceleration.  The  vehicle  oscillates  about  this 
mean  position  with  a  period  of: 

T  —  —\l^-  —  84  min 
2t r  V  g0 
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Thrust  acceleration  in  the  ^-direction  greater  than  g0  causes  exponentially  increasing 
velocity  and  position. 


C.  ERROR  ANALYSIS 

Linearize  eq.  5.3  at  (x0,y0,  z0)-, 


Therefore: 


8x  =  —j^8x  +  8Ax 
8y  =  -j^8y  +  8Ay 
8z  =  2jf-8z  +  8Az 

■Tin 


(5.4) 


8x 

R0 

8z 

R0 


8 Ax  (  !~9o~ 

9o  \  \J  R0  )  Ro 


Therefore,  errors  in  x  and  y  are  sinusoidal,  but  errors  in  2  increase  exponentially 
with  time.  =>  do  not  use  dead  reckoning  to  compute  z. 


D.  THE  VERTICAL  CHANNEL 

•  Integrating  acceleration  in  z  leads  to  divergent  solution 

•  Use  altimeter  measurements  together  with  az  to  obtain  altimeter  data. 

•  Altitude  is  computed  in  CADC  (Central  Air  Data  Computer)  based  on 

—  Tfat  =  free  air  temperature 
-  Ps  =static  pressure 

Definition’s 

•  Absolute  altitude  ( Habs )  Height  above  the  earth  surface  at  a  given  location. 
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absolute 

altitude 


Mean  Sea  Level 

Figure  5.3:  Altitude  Definitions  [Ref.  1,  p.  199] 


•  True  altitude  (Htrue)  Actual  height  above  the  standard  sea  level. 

•  Pressure  altitude  ( Hp )  Height  in  model  atmosphere  above  pressure  datum  plane 
of  29.92  in.  (760mm)  of  mercury.  (Does  not  consider  variations  in  pressure  and 
temperature  at  sea  level,  unlike  true  h ). 

•  System  altitude  Computer  corrected  Hp  for  non-standard  day  variations. 

ICAO  Standard  Atmosphere  [Ref.  1,  p.  201]. 

•  Earths  atmosphere 

—  Troposphere:  Lowest  layer.  Characterized  by  decrease  in  atmosphere  (neg¬ 
ative  lapse  rate).  Contains  most  of  air  and  moisture. 

-  Stratosphere:  Lapse  rate  changes  direction.  Marked  decrease  in  water 
vapor. 

-  Metosphere:  Lapse  rate  again  reverse  and  T  decreases  with  altitude. 


-  Thermosphere:  Above  80km,  air  temperature  decreases  with  altitude.  Gas 
molecules  are  separated  by  large  distances. 

E.  ATMOSPHERIC  MODEL 


P  =  pressure 
Z  —  geometric  altitude 
dP  —  —pgdz 


Where 


9 

9o 

P 


9oPj 
(Re  +  Zf 

9.87m/ s2  (32.2ft/ s2) 
atmospheric  density 


From  the  ideal  gas  law 


Where 


MP 

9  ~  JFT 


M  =  mean  molecular  weight  of  the  air 
R*  —  universal  gas  constant 

T  =  absolute  temperature 


dP 


gM 

R*T 


dz 


=>  dlnP  =  ~^=dz 
R*T 

=>•  P  =  P0  exp  -^^-z 
y  R*T 


(5.5) 


(5.6) 


51 


Where  PQ  =  pressure  at  sea  level 
Similarly 

P  =  po  exp 


g_M 

R*TZ 


Let 


r 


Then  for  a  given  layer 


dT 

dz 


(lapse  rate) 


T 

T 

P 

P 


T0-Tz 

_dT 

dz 


Also,  from  US  atmosphere  model,  we  get: 


T  =  T(HP  -  H0)  +  T0 
and  from  Equations  5.5  and  5.6 

T  (P-  -  -  i) 

h,  =  g.  +  Ak-  j-a  Tr# o 

B,  =  B0- ^Aln(i>a),r  =  0 

9  0**1 

Where,  T0,H0,P0  are  obtained  from  US  Standard  Atmosphere  Model.  Numer¬ 
ical  examples  [Ref.  1,  p.  204], 
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F.  VERTICAL  CHANNEL  DAMPING:  COMPLEMENTARY  FILTER¬ 


ING 

Idea:  Use  altimeter  information  in  steady  state  and  at  low  frequencies  (altitude 
is  noisy).  Use  acceleration  data  at  higher  frequencies. 

How?  Consider: 
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Figure  5.4:  Complementary  Filter  Mechanization 


Consider  Figure  5.5  (see  Figure  4-16  [Ref.  1]).  We  can  write  the  following: 


Figure  5.5:  Complementary  Filter:  ^-Channel 


(from  CADC) 


ii  =  xz  -  c2(hs  +  x2) 
x2  =  X\  —  C\(hs  +  x2) 


x\  =  —  c2x2  —  c2hs  4-  xz 

X2  —  —C\X  2  +  X\  —  Cihs 


In  state  space  form  we  have: 


0  —c2  xi  -c2  1  [  1  1  .. 

1  -ci  x2  -ci  0 


h  =  xz  =  0  1 


In  transfer  function  form,  h(s )  =  C(sl  —  A)_1i?  we  get: 


h(s)  =01 


5  C2 
-1  s  +  cx 


Taking  the  inverse  of  (SI  —  A): 


si -A)-1  =  [  0  1 


5  C2 
—  1  s  4-  Ci 


_C2  L  |  1  •• 

-Cl  +  G  ^ 


1  5  +  Cl  C2 

S2  +  CiS  +  c2  1  s 


Substituting  5.9  into  5.8  and  multiplying  through  gives: 

1  -  [0  1 1  A  -(3  +  cOc  -  Clc2 1  f ,  +  Cl  1  \ 

S2  +  CXS  +  C2  L  J  \  [  -C2-SC !  J  1  J  J 


-(aci  +  ca)  h  1 

-S2  +  Cl  5  +  C2  32  +  Cj5  +  C2 


Compare  with  our  filter: 


Now  add  a. 


Ci  =  a,  c2  =  b 


xi  -  az  +  lu\x2  -  c2(hs  +  x2) 
x2  =  xi  -  a(hs  +  x2) 


(5.10) 


ii  =  (— c2  +  2cu2)x2  -  c2hs  +  az 

x2  =  — C\X2  +  Xi  -  C\  hs 


(5.11) 


„  /  xi  — c2  ,  1 

i2  ~  °  -C2  +  H  X2  +  _Cl  hs+  0  xz 

j  L  l  -ci  I  L  J  L  J  J 


h  =  x,  =  0  1 


This  gives  the  transfer  function  h(s ): 


■h(s)=  zk*+M  hi+ 

52  +  Ci5  T  C2  52  T  Ci5  -j-  C2 


Where: 


Substituting: 


a  =  ci 


b  =  c2  —  2tu2  =  b  —  2u>2 


as  +  b  —  2cu2  1  ,  , 

a{s)  2  T  "T  7  n  2  ^  2  i  i  1  O  2  '  z  So) 

sz  -f  as  +  b  —  2u;f  s2  +  as  +  b  —  2tu2 
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Note,  in  steady  state  az  must  equal  g0.  Suppose  az  measurement  has  errors,  i.e.  let 


O Z  Q*ZO  8a Z1  O zo  —  Qo 

and  suppose  6azo  ^  0  in  steady  state.  Compute  h  in  steady  state. 
Using  the  Final  Value  Theorem: 

A(0)  =  lims/i(s) 

=  lim a  7\ ( s )  hs(s )  +  lim a  T2(s)8az(s) 

Where: 

r.W 

2iW 

Suppose: 

8az  =  const2  =>  <5a2(s)  =  C°nS^2 

s 

i  ,  i  /  \  const  x 

hs  =  consti  =$■  h$(s)  =  - 


as  T  b  — 

s2  +  as  +  b  —  2 u)2 

_ 1 _ 

s2  +  as  +  b  —  2u2 


=>h 0)  -  Ti(O)MO)  +  T2(0)8az(0) 

—  hs(0)  +  t_  2  2^(0) 

^  s 


Note,  we  want  A(0)  =  As(0). 
Consider 


h(s)  = 


s3  +  as 2  +  6a  + 
a3  +  as 2  +  bs  +  c  s 
a3  +  6a  +  c 

s3  +  as2  +  bs  +  c  5  5 


as 


s3  +  as2  +  bs  +  c 


7  <5a2(s) 


tl{s) 


Ms 
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Where 
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VI.  KINEMATIC  EQUATIONS  AND  ERROR 

ANALYSIS 


A.  ROTATING  COORDINATES 


Q 


Figure  6.1:  Rotating  Coordinates 


%=  %,+  iC’PQ  (6,1) 

Suppose: 

%Pop  =  0  and  pPq  =  const 

Then: 

{Pq  =  ;c  ppQ 

Differentiating: 

|  %  =  ±P’pQ  =  ±<frm, 
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Note 


=  ;cs(pnpi) 

=  ,) 

=  to  fcsc'sv)  fc] 


=  si‘nri);c 


•at,  x  -pQ 


=  to  X  Pc 


Now,  suppose  pPq  /  const.  Let  |-  iPg  =  vector  of  time  derivatives  of  each  element  of 


Then: 


±%  =  1%+‘»X'P0 


In  general,  Coriolis  theorem: 


d  .  6  A 

ItA=TtA  +  u,xA 


Now,  let’s  compute: 


it*  'Pq  ~  7t(lt'F,})  +  Tt('u’><  ‘J5«) 

=  tz%  +  4"  x  t:  %  +  Y,  (b  x  >pq)  +  ’ 


u>  x  u;  x 


Note: 


(a  x  b)  =  a  x  b  +  a  x  b 
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Therefore: 


St  M  x  'Pq  ~{ltUJ)x  'Pq  +  'uX  It  Pq 


d  ■  8  ■  <  A  S  ■ 

—  ‘w  =  —  ‘w  +  *W  X  tU=—liO 

dt  St  St 


Therefore,  we  get: 


dt 2  Pq  ~  St 2  Pq  +  b  x  St  Pq  +  ‘"J  x  8 Pq  + 

£ 

!W  x  —  *Pq  4-  SCJ  X  (w  *>g) 

=  h'PQ  +  {lt'“)  x  '•pa  +  2‘“'x^i-P«+  (w  •>« 

Notation  =  i: 

Now,  suppose  iP0p  7^  0,  then 

£2  £ 

=  *>0,  +  ^2  %  +  %  X  Tq  +  2  ^pi  x  -  %  +  %i  X  (  %i  X  ‘Pq 


Using  the  notation  in  Siouris  [Ref.  1], 


r  =  R  +  p 


Then: 


?  =  &  +  +  l&pi  XP  +  %i  x  (  % Pi  x  p)  +  2  *Qpi  x  ^ 
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Where: 


IC'S 


IC'S 


Figure  6.2:  Calculation  of  Velocity  and  Position 
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V 
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V- 

a 

R 

Jn 

Pn  (sin  <f>) 

t 

R 


earth’s  gravitational  constant 
mean  equatorial  radius  of  earth 

\Jxl  +  yl  +  zl 

latitude  (sin  <j>  =  -p~) 

R 

coefficients  of  zonal  harmonics  (constants  determined 
from  observations  of  orbit  perturbations  of  artificial 
satellites) 

associated  Legendre  Polynomial  of  the  1st  kind 
potential  of  spherical  mass 
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The  rest  account  for: 


*  Earth  is  bulged  at  the  equator 

*  Flattened  at  the  poles 

*  Generally  asymmetric 

Note,  if  symmetry  wrt  equator  is  assumed  =>  odd  harmonics  vanish. 
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Therefore: 


=  nA_  (r^ne  +  2^e.)  X  nV  +  ng(R)  (6.14) 

Jp 

from  pg.152 

^  VE  A-e  0  —  lojj  —  2,£lg  u>n  +  2  Ve  gx 

—  VN  =  AN  -  u>u  +  2 flu  0  -uE  -  2 VN  +  gy 

.  Vu  .  .  A u  .  .  -u/v  —  2 S7jv  me  +  2£lE  0  Vv  gz 


= 

_ o 

$ 

w 

II 

f  VN  dt 
Jo 

Vu  = 

f*  Vu  dt 

Jo 

In  {n}  for  spherical  earth: 


0  ]  [  0 
0  ~  0 
gmo(ji)2  _  .  9mo(  1  -  j^) 


<t> 

A 


Vn_ 

R4,  +  h 

Ve 

(R\  +  h)  cos  <f> 


Figure  6.3  shows  the  standard  mechanization  for  Equation  6.14. 
Wander  Azimuth  Mechanization 


p 
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—Sea.  +  A  c<bsa 
<j>sci  +  Xcd>ca 
a  +  A  s<p 

ca  sa  0  ]  [  0 

—sa  col  0  flcos 

0  0  1  H  sin  < 

VlcQsa 

riccpca 

Cls6 

ca  sa  0  1  I"  Ve 

—sa  ca  0  Vjv 

0  0  1  J  L  Vu  _ 


ca  sa  0 
— sa  ca  0 
0  0  1 


R<f>  +  h 

i  Vg 

( R\  +  h)c(j) 

a  =  —A  sin  <f>  (wander  azimuth) 
Alternatively,  compute  A,  <f>,  and  a  from  direction  cosines: 


ic=*£s(mne) 

Integrate  Equation  6.15  to  get  PC.  Then,  for  { n }  =  ENU: 


(6.15) 


Where 


cacX  —  sascjxsX 

sac<j) 

—casX  —  sas<f>c\ 

— sacX  —  cascf)sX 

caccb 

sasX  —  cas<f>cX 

c(f>sX 

S(f> 

c(j)cX 

Cu 

C12 

C\3 

V-  c21 

C22 

C23 

C31 

C32 

C33  _ 

69 


And 
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Now  wander-azimuth 


PQpez  —  0  =>  a  =  —\s(f> 

—<j>cot  +  A  c(j>sa 
=>  ^pe  =  <f>sa  +  A c4>ca 

Xscj)  +  o.  =  0 

3. 

^  =  M-(^pe  +  2^e!)  x  ’V  +  g(R) 

\v 

4. 

Vn 

R<t>  +  h 

Ve 

(R\  +  h)c6 

—\s<t>  (for  wander-azimuth). 


<p  = 

A  = 

a  = 
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*  Governing  equation: 


4i?  =  ;cpA-h  ig=  p(pA  +  pg) 


z 


Figure  6.4:  Latitude  and  Longitude  in  {?}  Frame 


Therefore 


{R  = 


(. R  +  h)c4>cA 
( R  +  h)c<j>sA 
( R  T  h)s<p 


=>  A  =  tan  1  ^ \  =  A  —  \0  +  fi 


=>•  A  =  A0  +  tan 


->  (% 


<& 1  +  M 


.  .  (  ‘R,  ' 

o  —  sin  — — - 
Y  V  'R  +  h 


+*  0ej-  x  *R 


Recall 


Then 


nV  =  sc(")  =  fix  *R) 

*  Compute  nV : 


C<f) 

0 

s<p 

cA 

sA 

0  ' 

?c  = 

0 

1 

0 

— sA 

cA 

0 

—  S<j) 

0 

C<f) 

0 

0 

1 

ctficA  ccpsA  scj) 
—sA  cA  0 
—s(j)cA  —s(f>sA  c<j> 


*  Finally,  ‘  C  =  const  =  initial  values 


Figure  6.5:  Navigation  Mechanization  PA  — *  nV 


Assume  ellipsoidal  earth 


*  Torquing  =>•  the  same 


Governing  equation: 


'R 

lR 

A 

A 


=  tan 


=  sm 


;CpA+  >g 

( R\  -f  h)c<j>c\ 
(R\  +  h)c<f>s\ 
[(1  —  e2)R\  +  h ] 

\RJ 

Xa  -)-  A  —  £lt 

iRz 


or  consider 


,(1  ~s2)Rx  +  h/ 


( 'Rl  +  =  ((i?A  +  hf  cos2  *)  7  =(RX  +  h )  cos  cf> 

1R* 

=£>  tan  6  =  - .  . 

+  •'*? 

Example  Wander  Azimuth  Navigation 
{n}  =  tAEJV 

Vehicle  is  traveling  in  a  plane  defined  by  (f>0  =  45'  with  the  equitorial  plane.  Vehicle’s 
velocity  V  along  it’s  path  is  400^i.  Vehicle  has  wander  azimuth  system  aboard. 

1.  What  is  71 VI 


1 

o 

_ 1 

1 - 

o 

II 

e 

400 

0 

— 

178— 

sec 

0 

2.  What  is  nOne,  nflei,  nftri 


0  ] 

0  1 

eO  — 

une  — 

0 

= : 

0 

V 

-  (R  cos  4>0)  - 

A 
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Figure  6.6:  Wander  Azimuth  Navigat 


10 


Now 


n 


^ ne 


C(f>0  0  S(j)0 

0  1  0 
.  —S<Po  0  c4>o 

Xs(po 

0 

Xc(f>0 

tan  (po 

0 

v 

R  J 


cA  sX  0 
— sX  cX  0 
0  0  1 


Check  Figure  6.7 


and  solve  for 


Figure  6.7:  U  and  N  Transformation 
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Platform  torquing: 


f  uxc  =  Cts<f>0  <—  wander  azimuth 

^yc  ^~R  ~ f^n^o)sO 

=  (^4 -  Vls(f>0)sa 


0JZ 


4.  What  do  accelerometers  measure? 


'd2R' 


dt 2  , 

'  /  ^ 

P9m(R)  = 


_J£_  ■ 

r  R  “| 

R 2 

(i?o+/l)2 

0 

= 

0 

0 

0 

5.  What  is  (^r)^  in  terms  of  nV,  pDne,  pDei? 
Recall 


' dR ' 


'dR 


Differentiating: 


dt)i  ~  \dt)e  +  n-XR 
—  V  +  Q,ei  x  i? 


d  =(^Pl  +tteiX(V  +  neixR) 


dt 2 


Applying  the  Coriolis  theorem  to  ^jjQ  .  and  expanding  the  cross  product  terms, 
we  get: 

(d2R\  (dV\ 

y  dt 2  J  —  y  ^pi  x  ^  "1"  x  A  +  Dei  x  (Dei  x  R ) 

^  ~dt  ^  (^Pe  ”h  ^e»)  x  V  +  Dei  x  V  -}*  0ej  x  (Dei  x  i?) 

—  +  ^Pe  "i~  2Dei)  x  V  +  Dei  X  (Dei  X  1?) 

=  +  (Dpe  +  2Dej)  xV  +  Dei  x  (Dei  x  V),  where  Dei  «  0 

-  (Dpe  +  2Dei)xV+^Q  (6.16) 


77 


D.  ERROR  ANALYSIS 

*  Idea:  Perturb  the  navigation  equations  around  the  true  values  of  the  position 
and  velocity  vectors.  Consider  tangent  plane  mechanization,  Figure  6.8. 


Figure  6.8:  Tangent  Plane  Mechanization 


Let  V  =  r 

Then  V  =  A  +  g(R ) 

f  r  =  V 

*  X  v  =  g(R)  +  A 

From  Figure  6.9  R  =  tR  —  tRsc  +  V 


Therefore  we  obtain 
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Figure  6.9:  Rec 


Now,  suppose  *pC  is  known  exactly  but, 

r  =  r0  +  6r 

V  =  Vo  +  SV 
lA  =  An  +  6  A 


(6.19) 


Where  r0,  V0,  A0  are  true  values  and  8's  are  small  errors.  Now  let’s  linearize  6.19 
around  r0,  V0,  A0  to  obtain  a  linear  model  of  the  error  dynamics. 


8V  =  6 A  +  [  &&  ]  8y 

6z 


Where 


8r  =  8y  ,  8V  =  8Vy 


'  8ax  : 

8  A  =  8  A, 


Consider 


flX 

ll«ll3 

!I«IL3 

yfz+Ro 

IW 


same  for— g(R) 


_ 0-  u  s+Bx 

dzV  ||B||3 
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Where 


x  —  state  vector  consisting  of  vehicle  position  and  velocity 
u  =  accelerometer  inputs 

Now  one  can  be  used  to  obtain  error  equations  for  INS  mechanization. 


Let 


x  =  x0  +  8x 
u  —  u0  +  8u 


where  x0,uo  are  true  values  and  8x,8u  are  errors. 
Then 


8x 


df  ,  ^  d f 


8u 

o 


A8x  +  B8u 


(6.22) 


Equation  6.22  describes  error  dynamics  of  the  navigation  equation  6.21.  Now,  let’s 
consider  the  vertical  channel  equation  in  the  tangent  plane.  The  mechanization  is 
shown  in  Figure  6.10. 


Figure  6.10:  Tangent  Mechanization:  Vertical  Channel 
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then  from  Equation  6.20  we  obtain: 


f  Sz  =  Svz 
Svz  =  jj^Sz  4-  SAZ 

The  mechanization  for  the  vertical  channel  error  equations  is  shown  in  Figure  6.11, 


Figure  6.11:  Vertical  Channel  with  Error 


Sz(s)  _  1 

£-Az(s)  s2  +  ^ 

where  the  poles  are  ±\J^  which  show  that  the  errors  in  the  vertical  channel  blow 
up.  Therefore,  let’s  use  a  complementary  filter  to  stabilize  the  errors,  Figure  6.12. 


Figure  6.12:  Vertical  Channel  Complementary  Filter 


Here  6  hs  represents  changes  in  hs.  Complementary  filter  equations: 
Sx i  =  —  a  6xi  +  Sx2  +  a  8hs 

<  5x2  —  —  (b  —  +  {b  —  ^)5hs  +  6AZ  Sx\  +  5x2  +  aShs 

k  Sz  =  Sx  i 
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—a  1  8x  i  a  1 

-b  0  (Xl  +  b  Sh‘ 


Sz=\l  olff1 

I-  j  0^2 


0 

1 


(6.24) 


Question:  How  do  we  implement  filter  6.24  with  the  nonlinear  Equations  6.23? 
Answer:  Replace  <5’s  with  full  scale  signals  in  Figure  6.11  and  ^  with  A* 


tT 

1/s 

X2  +  ^ 

7  /« 

JL/  S 

Xi  z 


P (Z+Ro) 

. 1  . 

II  ■Rll  3 

- — j - 

b 

Figure  6.13:  Linearized  Vertical  Channel  Filter 

Linearize  Figure  6.13  at  x0,  y0,  z0,  Azo  to  get  Equation  6.24. 


=  —ax q  +  x2  +  ahs 
x2  =  —bx  i  +  bhs  + 

i  =  Xi 


i^  +  A, 


(6.25) 


X\  —  H b  bx\j  x2  S20  d-  bx2^  hs  —  ^50  “b  8h§^ . 


—  — aSx-y  -b  bx2  A  a6h$ 

6x 2  =  — b8x\  -b  b8hs  -b  jfi^bx  1  -)-  <5A^ 

Note  Equation  6.26  is  the  same  as  Equation  6.24. 


(6.26) 
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Summary:  Filter  design  process 


1.  Obtain  INS  mechanization  equations. 

2.  Linearize  around  true  values  of  INS  states  and  inputs. 

3.  Design  a  filter  for  the  linear  equations  obtained  in  step  2. 

4.  Implement  the  filter  on  the  non-linear  equations  in  step  1. 

Probability  and  Stochastic  Processes 

The  errors  in  accelerometers,  altimeters,  GPS,  gyros,  ...  are  usually  modeled 
as  random  signals  having  certain  statistical  (stochastic  properties)  properties.  This 
is  shown  in  Figure  6.14. 


Figure  6.14:  Random  Process 

Definitions 

•  Freeze  time:  x(ti )  is  a  random  number  with  values  in  some  interval  of  R  (real 
line). 

•  E(x(t))  =  average  (mean)  value  of  x  over  time. 

•  E[(x(t)  —  x(t))2]  =  variance  of  x(t)  ( rms 2). 
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•  £[(*(<)  -  s(<))(*0")  -  *(^))],  t  ±  t  is  called  covariance  of  x(t)  and  is  denoted 
by  Px(t,r).  Note:  variance  of  x(t)  = 


•  Given  x(t)  and  y(t):  two  random  processes,  we  define 

P„  =  £[(*(()  -  *(<))( »(t)  -  y(r))\ 

as  the  cross-covariance  of  x(t)  and  y{t). 

If  x(t)  is  a  vector  of  random  processes: 

Xi(t) 

x(t)  =  ; 

xn(i) 

■  E(xi(t))  - 

E{x(t ))  =  x(t)  =  j 

_  E(xn(t))  _ 

Px(t,r )  =  E[(x(t )  -  x(t))(x(T)  -  «(r))r]  <—  is  a  matrix 

(T  xx(<)  -  xx (t)  1  V 

=  E  :  «i(t)  —  ^i(t)  •••  *n(T)-xn(r) 

V  _  ^n(t)  _  J 

'  £[(*i(0  -*i(*))(*i(r)-  *i(r))]  •••  ^[(®i(t)-*i(t))(xn(r)-xn(r))]  ‘ 

-  E[(xn(t)  -  ®n(<))(*i(r)  “  ^i(r))]  ‘ - '  E[(xn(t)  -  xn(t))(xn(r)  -  *n(r))]  . 

P  is  a  n  x  n  symmetric  positive  definite  matrix  with  variances  on  the  diagonal  and 
cross-covariances  off  the  diagonal. 

Definition:  x(t)  is  called  wide  sense  stationary  ( wws )  if  x(t)  =  constant  and 

P(t,r)  =  £[(*(«)  -  x(t))(x(r)  -  x(r))T] 

=  P(*-r) 
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From  now  on  we  will  deal  with  E(-)  which  is  a  linear  operator: 

E(xi  +  x2)  =  E(x  2  +  Xi) 

E(  const)  =  const 

E(Mx)  =  ME(x),  where  M  is  any  constant:  scalar,  matrix 
E(xM )  =  E(x)  ■  M 

=>■  £[(a:(f)  -  x )(x(t)  -  x )r]  =  E[x(t)x(r)  -  x(t)xT  -  xx(t)t  +  xxT)] 

=  E[x(t)x(r)  -  E(x(t))xT  -  xE(x{t))t  +  xxT)) 

=  E^x^xir))  —  xxT  —  xxT  +  xxT 
=  E(x(t)x(r))  -  xxT 

If  y  =  Mx,  x  &  :  y  are  wss  processes  and  M  <E  RnXn  _  constant  matrix. 

Then 

y  =  E(Mx)  =  ME(x )  =  Mx 
Rv  =  M  Rx  MT ,  Rx  =  covariance  of  x 

Those  are  important  identities.  Suppose  x(tt)  is  independent  of  x(t2 ),  x(t3),  •  •  • ,  x(tn ) 

for  all  <!,••• ,  tn. 

Then 

£’[(x(t1)  -  x)(x(t2)  -  x)T]  =  E[s r(<i)x(<2)]  “  xxT 

=  E(x(ti))E(x(t2))  —  xxT 

=  0,  V<i,<2 

Note,  for  independent  random  wss  process  x{t ), 

Rx(t,r)  =  :  '•  : 

£?[(*„(<) -*B(0)(*i(r)-*a(r))]  •••  E[(xn(t)-xn(t))(xn(r)-xn(r))} 

R,  t  —  T 
0,  t  ±  0 

=  R6(t  -  r),  is  symmetric,  positive  semi-definite  matrix 


87 


Definition:  x(t)  is  white  gaussian  noise  if  it  is  independent  and  for  each  fixed 
timet,,  x(ti)  has  gaussian  distribution  /(x(t,)),  Figure  6.15. 


Here  £  is  a  scalar  random  process. 


^rxp 


^  1  (a(fr)  -  xf  ^ 


in  the  matrix  case: 


/(*(<*•)) 


(2tt )ndet  Px 


exP  -  x)Tpx  1(x(ti) 


If  x(t )  is  white  gaussian  we  write: 


x  ~  N(x,  Px ) 


Finally,  consider 

Then 

& 


is  a  solution  of: 


x  =  Ax  +  Bw,  w  ~  N(0,  Rw) 

x(t)  =  E(x(t))  =  eA^E(x(t0)) 
Px(t)  =  F’[(s(t)  -  x(t))(a:(t)  -  z(t))T] 
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Px{t)  =  A  Px(t)  +  Px(t)  At  +  B  Rw  Bt 
If  A  has  all  A’s  in  left  half  plane,  then  Px( oo)  =  Px  (steady  state  covariance) 
0  =  A  Px  +  Px  At  +  B  Rw  Bt  (Lyapunov  Equation) 
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VII.  NAVIGATION  USING  NAVAIDS 


Purpose:  Compute  vehicle’s  position  and  velocity  using  navaids  and  determine 
how  accurate  these  computations  are. 

Let  x  e  Rn  be  the  vehicle’s  position,  (n  =  3).  Let  y  <E  Rp  be  the  vector  of 
navaid  measurements  p  <  n  =  3.  In  general  (by  geometry,  trigonometry,  ...)  we  have 


V  =  g{x) 

Let  x0,  y0  be  nominal  quantities  of  x  hy,  then 


=  Xo  +  8x,  y  =  y0  +  8y 


& 


8y  = 


dg 2 


dx 


8  x 


Equations  7.1  and  7.2  represent  p  equations  with  n  unknowns. 
Exj  Planar  navigation  using  bearing  fixes,  Figure  7.1. 


(7.1) 


(7.2) 


y\  =  arctan 
j/2  =  arctan 


S2-a  2 


x\  —d\ 

?2~h 


y  =  g(x) 


(7.3) 


Measure  yi,  y2:  assume  (ai,  a2),  (&i,  62)  are  known  and  solve  for  aq,  a:2.  In 
Equation  7.3,  yi  is  called  a  fix.  The  nature  of  the  surface  y,  —  const  determines 
the  nature  of  the  fix:  linear,  conic,  circular,  hyperbolic,  etc...  It  is  difficult  and  time 
consuming  to  solve  y  =  g(x)  directly.  We  need  to  come  up  with  an  interative  method: 
Let  p  —  n  and 


V  =  Vo +  8y 
x  =  x0  +  8x 
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Beacon  1 


Beacon  2 

9 


Vehicle  (Xi,X2) 


Figure  7.1:  Two  Bearing  Fix 

=$■  8x  =  C_16y  (if  inverse  exists) 
The  estimation  mechanization  is  shown  in  Figure  7.2. 


Figure  7.2:  Error  Estimation 


'  Sh  =  (| *)~T  Jyi  =  C-16yi 

Xi  —  X{~ ]  "I-  Sx{ 

.  Sy  =  yi-  g(xi) 


Estimation  Newton  Algorithm 
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Error  Analysis 

Usually  y  is  corrupted  by  noise 

y  =  g{x)  +  v  vector  of  measurement  noise, 


Assume  v  is  small  and  v0  —  0  :  v  —  8v 


Then 

6y  = 


and  the  estimate  error  ex  is: 


T 

Sx  +  v  =  C  fix  +  V 

0 


ex  =  8x  —  8x  =  Sx  —  C  1  8y 
=  8x  -  C'\C  8x  +  v) 

=  8x  —  8x  —  C~l  v 

=  -c"v=-$)  v 

Let  v  ~  N(0,Ry).  Then 

E^=E  ((!)’!;)  =° 

and 

Rex  =  c -1  Rv  C-T  =  ( CT  R-Vl  C)-1  (7.4) 

This  formula  allows  us  to  determine  the  best  combination  of  fixes:  “Rex  should 
not  be  much  greater  than  Rv.” 

Rule  of  thumb:  Have  the  fix  surfaces  as  orthogonal  as  possible,  Figure  7.3. 
Columns  of  C  will  become  orthonormal.  The  less  orthogonal  the  more  dependent  the 
columns  of  C  are,  the  greater  the  noise  amplification  in  7.4. 
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Note: 


d  ,  1 

—  tan  u  = - - 

du  1  +  u2 


%2  —  &2 

dxi 

(xx  -  aj)2  +  (x2  -  a2)2 

dy*  _ 

X\  —  CL\ 

dx2 

(#i  —  ai)2  +  ( x<i  —  a^)2 

C*> 

to 

I 

%2  —  &2 

dxi 

{x\  —  b\)2  +  (x2  —  b2)2 

dy2  _ 

X\  ~  W 

dx2 

(xj  —  bi)2  +  (x2  —  b2)2 

dg\T  [  |a-  fa- 

_ _  I  _  aari 

9*/  LI?  IS 

er  —  C-1n 


i?ex  =  c-1  ^  c-r  =  (cr  i?;1  C)-1 

Suppose  p  >  n  =$■  redundant  measurements. 

Given 

y  =  g(x)  +  v  (7.5) 

Question:  Do  redundant  measurements  help? 

First,  how  can  we  use  redundant  measurements?  Suppose  we  know  fv(v )  = 
probability  density  function  of  v.  Note,  the  values  of  v  for  which  fv(v)  is  large  are 
more  likely  to  occur  than  those  for  which  fv(v)  is  small: 

fv(v  =  a)dv  ~  P[a  <  v  <  a  +  dv]  (dv  is  small) 

From  Equation  7.5 

v  =  y  -  g(x) 

=£■  find  x  which  makes  fv(v)  =  fv(y  —  g(x))  large: 

maxxL(x )  =  maxxfv(y  —  g(x )) 
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Here  L(x )  is  called  the  likelihood  function  =>•  maximum  likelihood  method. 
In  practice  we  assume 

v  ~  N( 0,  Rv) 

=»  /v(t>)  =  —n===  =exP 

{2,k)p  det  Rv  1-  2  J 

It  turns  out  we  only  need  to  maximize 


1  T  r> — 1 

exp  --u  u 


£i(*)  =  \  [(y  - ^(a;))T  ^ 1  (y  - 


=  0  :  n  equations  and  n  unknowns 


Recall,  in  the  neighborhood  of  nominal  position 

8y  =  8x  =  C  8x 

'  '  o 


Consider 


Li(8x)  =  ~(6y  -  C  8x)T  Rv  1  (8y  -  C  6x) 


dLijSx) 

d8x 


d  ri 

d8x  .2 


(8yT  R-'Sy  -  6xT  C  R~ 1  Sy  -  6yT  R;1  C  Sx  +  8xTCTR-1C8x ) 


-{-ZCR^Sy  +  CTR~lC8x)  =  0 


=>6x  =  [CTR-lC\  1  CTR~l8y 


Compare  equation  7.6  with  equation  (6.47)  in  the  text.  For  Rv  =  I  this  is  the  so 
called  “least  squares  solution”. 

Example:  1-D  navigation.  Suppose  we  measure  position  x  with  two  errors. 


J  yi 

— 

x  +  Vi 

1  y% 

= 

X  +  v2 

■ 

l ' 

■ 

=►  y  = 

l 

X  + 
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Assume 


v  = 


V2 


N 


1  0 
0  3 


i.e.  j/2  is  3  times  less  reliable  than  y\. 
maxxL\(x )  - 


1 

1 

1 

H 

r 

■  1 

0  ■ 

-1  r 

2 

1 

CN 

0 

3 

2/i 

2/2 


(2/i  -  x?  +  ^(j/2  -  z)2 


a£i(g) 

dx 


■(2/1  -®)  -  ^(2/2  -x)  =  0 


X  = 


3  2/1  4-  2/2 


weighted  sum 


Where  more  reliable  measurements  are  given  more  weight. 
Note 


x  = 


(\l  1 


1  0 
0  1 


1\  -1 


4  \Vl  3V2 ' 


3  j 

3  2/1  +  2/2 


1  1 


1  0 
0 


Error  Analysis  ( p  >  n ) 


e*  =  Sx  —  <Sx 

=  &r-  [C'Ti?;1C']_1CTJR^i/ 

=  Sx  -  [ctR^1c]~1  CtRv  (C6x  + 17) 

=  -\cTR-vlcYlCTR-v\ 

ex  =  [C'ri?“1C']_1  CTR~lv  =  Mv 
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rHfCO 


MRvMt  =  [CTR-Ic]  'c'TR-'R.Rz'C  \ctr-1c\  1  =  [ctr-'c] 


Example 


Rv  = 


M  = 


1  °1  r-  f  1 

0  3  J  ’  [  1 


3  1 

4  4 


Rex  =  t  *  4  j  0  3  \ 

_9_  3_  _  3 

16  +  16  "  4 


Compare  this  with  noise  for  y\  :  Rv\  —  1  k,  j/2 :  iU,2  =  3. 

We  do  better  by  using  both  signals. 

Use  Maximum  Likelihood:  The  mechanization  is  shown  in  Figure  7.5. 


Figure  7.5:  Maximum  Likelihood  Mechanization 

C  —  (If)  |_  :  %  =  Vi  —  Vi] 

<  Sxt  =  \CTR?c]~l  CTR-Hyi] 
ii  =  x~l  +  Sii 
\  Syi  =  g(xi) 

Error  Model  for  Wander  Azimuth  Navigation 


°R  =  vehicle  position  in  {e} 


Then 


Now,  let 


ev  +  ene{  x  eR 

epCpV  +  eQei  X  eR 

pa  +  pg(  eR )  -  ( pnpi  +  pnei)  x 

-  ( pnpe  +  2  pnei)  x  PV  +  M  +  pg(  eR) 


(7.7) 


pv  =  pv0  +  psv  =  pva  +  sv 

eR  =  PR0  +  p«5i?  =  ei?0  +  SR 
pA  =  pA0  +  P6A  =  M0  +  <5b4 

Note,  given  a,  6,  c,  €  i?3  and  c  =  a  x  6,  then 

5c  5(a  x  6)  d(-b  x  a)  dS(-b)a 

5a  “  a  “  5a  ~  5a  =  5(_6) 

and 

5c  _  5(a  x  6)  _  dS(a)b  _  of  ^ 
db  ~  db  ~  ~~db~  ~  ^ 

Therefore,  we  get 

57?  =  JCW+S(  «(!„■)  M2  _ 

dv  =  -s{pnpe  +  2pnei)sv  +  sA  +  ^§^-sR  {  ’ 

Note,  in  equation  7.8  (  )  was  used  for  differentiation.  This  is  abuse  of  notation. 
However,  since  we  integrating  both  sides  which  are  resolved  in  correct  coordinate 
systems,  it  is  OK. 

Recall, 

p9('R)  =  -jp’,iR  =  -jpr'C> 

The  vector  ir  is  shown  in  Figure  7.6. 
where 
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Figure  7.6:  Unit  Vector,  ir 


JL 
'  R* 


o 

1 _ 

1 

o 

_ 1 

1 

o 

_ 1 

1 - 

1 

1 _ 

to 

II 

-1 

0 

0 

A* 

i?3 

= 

0 

l 

o 

0 

f— 1 

[ _ 

I 

o 

_ 1 

- 1 

o 

_ 1 

Therefore,  we  get 


d^R) 

deR 


Finally,  we  obtain 


dpCpg(eR) 

deR 


pC 


peC 


d^gj  eR ) 


deR 

—  22. 
R0 

0 

0 


0 

_ 

Ro 

0 


0  ■ 

0 

_ 2°. 

Ro  J 


d_ 

dt 


'  SR  ' 

SV 

s(enei ) 


ic 

-s(pnpe  +  2pnei) 


SR 

sv 


+ 


I 

0 


SA 


We  should  be  able  to  verify  this  model  in  Simulink. 
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VIII.  THE  NAVSTAR  GPS 


The  NAVigation  Satellite  Timing  And  Ranging  Global  Positioning  System  is  a 
satellite-based  radio  navigation  system  with  the  capability  to  provide  locating  data 
to  an  unlimited  number  of  users  1.  The  first  satellite  was  deployed  in  1978  although 
the  first  receiver  did  not  become  commercially  available  until  1982  [Ref.  10,  section 
1.2].  This  system  is  the  product  of  experience  gained  from  several  previous  space- 
based  navigation  systems  like  TRANSIT  AND  USAF  System  621B.  It  is  comprised 
of  three  segments: 

•  Space  segment 

•  User  segment 

•  Control  segment 

Each  segment  is  discussed  in  the  following  sections. 

A.  SPACE  SEGMENT 

A  total  of  24  satellites  now  constitute  a  fully  operational  space  segment.  Twenty- 
one  of  theses  space  vehicles  (shown  in  Figure  8.1  operate  continuously,  while  the  re¬ 
maining  three  act  as  orbiting  spares.  Today,  only  three  Block  I  satellites  remain  in 
orbit.  These  satellites  were  the  first  GPS  satellites  in  space.  They  were  launched  from 
1978  through  1985  [Ref.  10,  section  1.2].  Block  I  space  vehicles  weigh  960  pounds 
and  generate  420  watts  of  electrical  power  [Ref.  8,  p.  27].  The  remaining  satellites  in 
orbit  were  creatively  named  the  Block  II  vehicles.  These  vehicles  are  less  vulnerable 

to  radiation  and  have  more  memory.  With  this  increase  in  capability  has  come  an 
'This  chapter  is  an  abridgement  of  Marquis  [Ref.  5,  Chap.  2]  and  is  included  for  continuity. 
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increase  in  weight  and  power  requirements.  These  newer  satellites  weigh  2000  pounds 
and  generate  700  watts  of  electrical  power. 


The  space  vehicles  are  inserted  into  orbits  defined  by  the  six  Keplerian  constants: 

•  semi-major  axis,  a 

•  orbital  eccentricity,  e 

•  orbital  inclination,  i 

•  ascending  node,  fi 

•  argument  of  perigee,  ui 

•  time  of  perigee  passage,  T 


Figure  8.1:  A  NAVSTAR  GPS  Satellite  from  [Ref.  5,  p.  4.01] 
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The  semi-major  axis  of  a  GPS  satellite  orbit  is  nominally  26,560  kilometers.  It 
is  half  of  the  length  of  the  ellipse  which  defines  the  space  vehicle’s  path.  Second,  the 
eccentricity  (different  from  the  eccentricity  defined  with  respect  to  the  shape  of  the 
WGS-84  ellipsoid)  or  oblateness  of  the  satellite’s  orbit  is  defined  as  follows: 


r_a  -  r_p 
ra  +  rp  ’ 


(8.1) 


where  ra  is  the  apogee  radius  and  rp  is  the  perigee  radius.  For  GPS  satellites,  the 
eccentricity  cannot  exceed  two  percent.  The  third  of  the  Keplerian  elements  is  orbital 
inclination.  It  is  the  angle  between  the  plane  defined  by  the  orbit  and  the  equator. 
For  example,  satellites  in  a  polar  orbit  have  a  90°  orbital  inclination;  those  in  an 
equatorial  orbit  have  0°  orbital  inclination.  Block  I  NAVSTAR  GPS  vehicle  orbits  are 
inclined  at  63°  while  Block  II  satellites  are  inclined  at  55°.  The  ascending  node  is  the 
satellite’s  geodetic  longitude  as  it  passes  through  0°  of  latitude  toward  the  northern 
hemisphere.  GPS  satellites  orbit  in  six  different  planes.  Thus,  there  are  exactly  six 
different  ascending  nodes.  The  last  two  Keplerian  elements  are  the  argument  of  the 
perigee  and  the  time  of  perigee  passage.  The  argument  of  the  perigee  is  the  angle 
in  the  orbital  plane  between  the  ascending  node  and  the  closest  point  of  approach 
of  the  satellite  to  earth.  The  time  when  the  vehicle  reaches  this  point  is  the  time  of 
perigee  passage.  The  ranges  of  these  last  two  parameters  span  all  possible  values  for 
GPS  satellites.  That  is,  satellites  reach  their  perigee  at  all  different  times  of  day  and 
different  locations. 

Ideally,  the  six  Keplerian  elements  would  be  sufficient  to  define  any  satellite’s 
three-dimensional  position  and  velocity  vectors  for  all  time.  However,  the  orbits 
become  perturbed  by  lunar  and  solar  gravity  and  the  earth’s  equatorial  bulge,  as 
well  as  several  other  less  significant  effects.  Thus,  the  number  of  quantities  required 
to  fully  specify  the  position  and  velocity  of  a  satellite  in  a  real  orbit  is  increased 
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to  16.  The  extra  elements  consist  of  time  rates  of  change  of  the  ascending  node 
and  inclination,  i.  e.  Cl  and  i,  as  well  as  six  other  coefficients  which  account  for 
variation  in  the  earth’s  gravitational  field.  These  16  coefficients  must  become  part  of 
the  navigation  message. 

The  navigation  message  is  the  framework  in  which  the  GPS  satellites  broadcast 
their  data.  One  “frame”,  the  complete  message,  consists  of  five  subframes  of  300 
bits  each.  Subframe  one  contains  coefficients  used  to  correct  the  satellite’s  clock  to 
exact  GPS  time.  The  16  pieces  of  ephemeris  data  are  broadcast  in  the  second  and 
third  subframes.  Subframe  four  contains  special  messages,  ionospheric  correction 
coefficients,  and  coefficients  for  conversion  of  GPS  time  to  Universal  Coordinated 
Time.  Ephemeris  and  health  data  for  the  entire  GPS  constellation  is  transmitted  in 
subframe  five.  Because  of  the  volume  of  data  in  subframes  four  and  five,  both  must 
be  subdivided  into  25  pages.  Therefore,  it  takes  25  full  frames,  broadcast  at  the  rate 
of  30  seconds  per  frame,  or  750  seconds  (12.5  minutes)  to  receive  the  entire  navigation 
message.  Critical  navigation  data  -  ephemerides,  and  clock  correction  coefficients  — 
are  updated  every  frame.  Secondary  data  transmitted  in  subframes  four  and  five  is 
provided  primarily  to  assist  the  receiver  in  acquiring  other  satellites.  These  data  are 
not  intended  to  be  precise  so  the  lower  update  rate  of  once  every  12.5  minutes  is 
satisfactory. 

Satellites  use  a  pseudorandom  binary  code  superimposed  on  the  two  carrier 
frequencies  to  communicate.  The  two  frequencies  are  Ll  (1575.42  MHz)  and  L2 
(1227.6  MHz).  The  Ll  frequency  carries  both  the  C/A-  (coarse  acquisition)  and 
P-(precise)  codes,  while  L2  carries  only  the  P-code.  The  less  precise  C/A-code  is 
broadcast  at  a  rate  of  1.023  million  bits  per  second  (Mbps)  and  is  1023  bits  long. 
Therefore,  this  code  repeats  itself  every  millisecond.  The  C/A-code  is  unique  to  each 
satellite  and  does  not  change,  allowing  GPS  receivers  to  quickly  distinguish  between 
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space  vehicles,  even  without  access  to  the  P— code.  The  P— code,  being  more  precise, 
is  transmitted  at  10.23  Mbps  with  a  code  length  of  approximately  six  trillion  bits. 
This  code  takes  37  weeks  to  repeat.  Since  the  codes  are  reset  every  week  at  midnight 
between  Saturday  and  Sunday,  there  are  sufficient  “code  weeks”  available  in  the  P- 
code  such  that  one  can  be  assigned  to  each  space  vehicle  each  week.  Therefore, 
GPS  receivers  can  easily  distinguish  satellites  from  each  other  with  their  individual, 
weekly-assigned  P-codes.  The  remaining  code  weeks  are  available  for  uplink  from 
the  control  segment  to  the  satellites. 

B.  USER  SEGMENT 

The  many  thousands  of  GPS  receivers  constitute  the  user  segment.  The  re¬ 
ceiver’s  functions  are  to  receive  and  interpret  the  navigation  message  and  to  calculate 
and  output  position.  GPS  receivers  must  determine  the  time  the  navigation  message 
takes  to  travel  the  distance  from  the  satellite  to  the  receiver.  This  is  achieved  by 
autocorrelating  the  pseudorandom  binary  pulse  train  received  from  the  satellite  with 
the  one  in  memory.  A  typical  civilian  GPS  receiver  must  have  the  C/A-codes  for  all 
24  satellites  in  its  memory  (requiring  only  three  kilobytes).  As  the  pseudorandom 
code  is  received,  the  receiver  slews  its  code  until  the  result  of  the  autocorrelation 
function  jumps  to  one.  The  receiver  is  now  “locked-on”  to  that  code.  Multiplying 
the  length  of  time  that  the  receiver  must  slew  its  code  to  achieve  a  unity  correlation 
by  the  speed  of  light  yields  the  “pseudorange”.  This  is  not  the  actual  range  because 
the  offset  of  the  receiver  clock  is  uncertain.  When  the  receiver  can  lock-on  to  four 
satellites  and  thus  measure  pseudoranges  to  each,  is  three-dimensional  position  and 
clock  error  can  be  solved  for  by  inverting  this  set  of  four  equations 

/A  —  v  O^safi  ~  Xrcvr)2  +  {Usati  ~  Vrcvr )2  T  (Zsati  ~  ^rcur)2  +  cAt 
Xrcvr  Y  +  ( Vsat2  —  Vrcvr  Y  +  (Zsat2  ZrcvT  y  +  cAt 
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P3  \Z(*^5at3  %rcvr)  ”i“  ( Vsat 3  Vrcvr “1“  {^sats  % rcvr H” 


P4  ~  \f (Zsatt  ~Xrcvr )  2  +  (?/sa<4  J/rci>r)2  +  (Zsat4  —  ^rcur)2  +  cAf,  (8-2) 

where  pt  are  pseudoranges,  [x,ot.,  j/sati,  2sai;]  are  the  ECEF  coordinates  of  a  satellite, 
[x row,  Vrcvr ,2 'rcvr)  are  the  ECEF  coordinates  of  the  GPS  receiver,  c  is  the  speed  of 
light,  and  A t  is  the  receiver’s  clock  error.  Having  solved  this  set  of  equations,  the 
receiver  now  need  only  transform  the  solution  to  the  geodetic  system  and  display  the 
results. 

C.  CONTROL  SEGMENT 

The  GPS  Control  segment  is  responsible  for  generating  and  uplinking  clock 
correction  coefficients  and  ephemeris  corrections  for  all  satellites  in  the  constella¬ 
tion.  Five  control  stations  —  Hawaii,  Ascension  Island,  Diego  Garcia,  Kwajalein, 
and  Colorado  Springs,  Colorado  —  comprise  this  segment.  These  control  stations  are 
essentially  GPS  receivers  capable  of  constantly  tracking  all  satellites  in  view.  Ad¬ 
ditional  capabilities  include  highly  accurate  Cesium  clocks  and  recording  facilities. 
The  first  four  of  these  stations  track  all  satellites  in  view  and  record  pseudorange 
information  continuously.  This  data  is  sent  to  the  Master  Control  Station  at  Col¬ 
orado  Springs  where  it  is  processed.  When  all  four  monitor  stations  have  locked-on 
to  a  single  space  vehicle  simultaneously,  the  exact  inverse  of  the  standard  naviga¬ 
tion  problem  mentioned  in  the  previous  section  exists.  Since  the  exact  locations  of 
the  monitor  stations  are  known  and  their  clocks  are  extremely  accurate,  the  four 
unknowns  become  the  three  coordinates  of  the  space  vehicle  position  and  its  clock 
offset.  The  Master  Control  Station  calculates  these  quantities  and  from  them,  derives 
the  necessary  ephemeris  and  clock  corrections.  This  information  is  uplinked  to  each 
space  vehicle  at  least  daily. 
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D.  DIFFERENTIAL  GPS 

Although  GPS  alone  provides  highly  accurate  positioning,  it  can  be  made  still 
more  accurate  by  augmenting  it  with  a  differential  station.  A  differential  station 
is  merely  another  GPS  receiver  whose  exact  location  is  known.  When  this  second 
receiver  is  near  the  first  receiver,  both  are  subject  to  nearly  the  same  errors,  i.  e.  the 
same  local  atmospheric  properties,  nearly  identical  elevation  angles  and  propagation 
paths  to  any  given  GPS  satellite,  the  same  clock  errors  and  the  same  ephemeris 
errors  for  each  satellite.  By  employing  the  Pythagorean  theorem  on  its  position  and 
the  satellite  position  broadcast  in  the  navigation  message,  the  differential  station 
can  calculate  the  exact  range  to  that  satellite.  Meanwhile,  it  can  also  calculate 
pseudorange  in  the  standard  way  (see  Section  B.).  By  comparing  these  two  values 
for  each  satellite  in  view,  the  differential  station  can  evaluate  the  pseudorange  error 
to  each  satellite.  These  values  can  be  broadcast  periodically  to  be  used  by  receivers 
in  the  local  area  to  improve  accuracy.  By  using  the  differential  corrections,  GPS 
accuracy,  even  for  single  frequency  C/A-code  users,  can  be  improved  to  one  to  seven 
meters  rms  [Ref.  8,  p.  64]. 

E.  GPS  ERROR  SOURCES 

Despite  its  exceptional  accuracy,  GPS  is  subject  to  numerous  error  sources. 
Clearly,  the  major  error  sources  must  be  included  in  the  DGPS  model.  Error  sources 
are: 

•  atmospheric  delays 

•  Selective  Availability 

•  clock  differences 

•  ephemeris  error 
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•  multipath 


•  receiver  noise 

•  Dilution  of  Precision  (DOP) 

Each  of  these  error  sources  is  discussed  in  detail  in  the  following  sections. 

1.  Atmospheric  Delays 
a.  Ionospheric  Delays 

The  ionosphere  is  a  layer  of  charged  particles  between  100  and  1000 
kilometers  above  the  earth’s  surface.  These  particles  interact  with  the  transmitted 
GPS  signal  and  slow  it,  increasing  pseudoranges.  The  equation  describing  this  delay 
is 

40 

At=7p  TEC ■  (8-3) 

where  At  is  the  delay  in  seconds,  c  is  the  speed  of  light  (3  x  108  m/s),  /  is  the 
system  frequency  (1575.42  MHz  for  LI),  and  TEC  is  the  Total  Electron  Content 
(elect rons/m2)  along  the  signal’s  path.  The  TEC  is  strongly  effected  by  the  solar 
cycle,  season,  time  of  day,  and  latitude.  TEC  maxima  occur: 

•  daily:  1400  local 

•  seasonally:  spring  equinox 

•  solar  cycle:  every  11  years  (next  2001-2002) 

•  geographic:  20°  magnetic  latitude 

It  varies  ±  25%  rms  at  all  latitudes  during  daylight  [Ref.  10,  section  2.5,  p.  24],  The 
pseudorange  error  due  to  ionospheric  effects  can  be  as  great  as  40  meters  [Ref.  10, 
section  2.5,  p.  13]. 
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Figure  8.2:  Diurnal  Ionospheric  Delay  from  [Ref.  6,  p.208] 

The  algorithm  described  in  [Ref.  12]  which  removes  55-60%  of  the 
ionospheric  delay  is  based  on  Figure  8.2,  which  shows  the  typical  diurnal  variation  of 
the  ionospheric  delay.  The  “ACTUAL  DATA”  curve  shown  in  Figure  8,2  is  modeled 
with  the  “COSINE  MODEL”  curve,  a  half-cosine.  The  equation  of  the  model  curve 
is 

At  —  DC  +  A  cos  2-~it~Tp\  (8.4) 

where  DC ,  A,  7 P ,  and  P  (constant  offset,  amplitude,  phase,  and  period,  respectively) 
describe  the  diurnal  variation  of  the  ionospheric  delay.  DC  and  Tp  are  assumed 
constant  at  five  nanoseconds  and  1400  local  time,  respectively.  Amplitude  and  period 
are  each  modeled  as  four  term  power  series  as  follows: 

*  =  E».fi 

n= 0 

p  =  EMI,  (8.5) 

n— 0 

where  the  o:n’s  and  /%’s  are  constants  which  are  broadcast  in  the  GPS  navigation 
message,  chosen  based  on  the  day  of  the  year  and  average  solar  flux  over  the  past  five 
days,  and  (j)m  is  the  geomagnetic  latitude  of  the  ionospheric  subpoint.  The  ionospheric 
subpoint  is  the  intersection  of  the  line  between  the  space  vehicle  and  the  receiver  with 
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the  surface  at  the  mean  height  of  the  ionosphere. 


Next,  one  must  first  find  the  subtended  earth  angle  between  the  user 
and  the  satellite,  EA  (degrees) 

445 

^=dT20-4’  <*«> 

where  el  is  the  elevation  angle  of  the  satellite  with  respect  to  the  user  in  degrees. 
Knowing  EA,  the  geodetic  location  of  the  ionospheric  subpoint  can  be  approximated 


<f>I  —  4>rcvr  +  EA  COS  (IZ 

x  x  |  E  A  sin  az 

AJ  —  Arcvr  .  ■  , 

COS  (pi 


where  <f>  and  A  denote  geodetic  latitudes  and  longitudes,  respectively  and  az  is  the 
azimuth  of  the  satellite  with  respect  to  the  receiver.  Now  the  geodetic  latitude  can 
be  converted  to  geomagnetic  latitude  (the  required  quantity)  with  the  following  ap¬ 


proximation: 


<f>m  =  $1  +  11.6cos(Aj  —  291), 


where  all  angles  are  in  degrees. 


The  dimensionless  scale  factor  (SF)  which  scales  the  entire  delay  is 
SF  =  1  4-  2<^^)3.  (8.9) 

The  final  expression  for  At  is  a  three  term  Taylor  series  expansion  of 


Equation  8.4  is 


SF  ■  [DC  +  A(1  -  §  +  ^)]  for  |*|<f 
SF  ■  [DC]  for  I  x  |>  f 


for  |  x  |>  | 


where 


27T  (t  -  Tp ) 


(8.10) 


Due  to  the  25%  rms  variation,  the  error  is  modeled  with  a  25%  stan¬ 
dard  deviation.  Therefore,  the  random  part  of  the  error  remains  within  ±25%  of 
nominal  68%  of  the  time. 

b.  Tropospheric  Delays 

The  lower  section  of  the  atmosphere  also  causes  signal  propagation 
delays.  Typical  tropospheric  delay  is  approximately  two  meters  for  90°  satellite  ele¬ 
vation  (directly  overhead)  up  to  28  meters  at  a  five  degree  elevation  angle  [Ref.  13, 
p.  218].  In  this  application,  the  atmosphere  can  be  modeled  as  being  composed  of 
“wet  air”  and  “dry  air”.  Dry  air  is  responsible  for  90%  of  the  total  tropospheric  de¬ 
lay,  whereas,  wet  air  is  responsible  for  only  ten  percent.  While  the  moisture  content 
in  the  troposphere  is  virtually  impossible  to  model  accurately,  this  inaccuracy  has 
minimal  impact.  Numerous  models  which  calculate  the  tropospheric  delay  have  been 
developed.  Black  developed  the  following  model  in  [Ref.  14].  Let 


As 

= 

Asd  “f" 

where 

A  sd 

= 

2.343P-[^T_4'12^ 
l  T 

A  sw 

= 

hyj  I \h  “  hyj) 

I{h,E) 

— 

M  r  cos E 

{  L(i  +  (i -Q- 

h d 

= 

148.98(T  -  4.12)  m, 

h>w 

= 

13,000  m, 

h 

ruw 

— 
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rs 

= 
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P 

= 

1  atm, 

T 

= 

15°  C, 

]-I(h  =  hd,E), 


xn(-1/2). 

rs 


(8.11) 
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and  As  is  the  wet  or  dry  delay  in  meters,  rs  is  the  distance  from  the  center  of  the  earth 
to  the  station,  P  is  the  surface  pressure  in  atmospheres,  E  is  the  satellite  elevation 
angle,  and  T  is  the  surface  temperature  in  degrees  Celsius.  It  should  be  noted  that  lc 
is  an  empirical  constant.  The  value  of  0.85  is  only  valid  for  elevation  angles  above  five 
degree  (GPS  receivers  typically  ignore  satellites  at  lesser  elevation  angles).  Similarly, 
kw  is  an  empirical  constant  which  varies  based  on  latitude  and  season.  The  value 
0.20  corresponds  to  the  value  for  spring  or  fall  in  mid-latitudes.  This  model  has  been 
shown  to  be  virtually  exact  at  elevation  angles  greater  than  40°,  with  its  worst  error 
of  about  0.045  m  occurring  between  five  and  ten  degrees  of  elevation. 

This  tropospheric  model  is  assumed  to  vary  15%  from  the  nominal 
value.  Therefore,  it  is  modeled  with  a  7.5%  nominal  standard  deviation.  This  main¬ 
tains  the  random  part  of  the  error  within  15%  of  the  model  value  95%  of  the  time. 

2.  Selective  Availability 

Selective  Availability  is  a  method  that  the  Department  of  Defense  can  use 
to  intentionally  degrade  the  accuracy  of  pseudorange  measurements.  Typically,  this  is 
accomplished  by  dithering  the  space  vehicle  clock  signal.  Dithering  the  clock  involves 
encoding  the  binary  time  signal  the  space  vehicle  broadcasts.  The  decryption  process 
is  classified  and  available  only  to  DOD  authorized  users. 

The  use  of  SA  essentially  results  in  the  satellites’  “lying”  to  the  receiver 
about  their  position.  Clearly,  this  adversely  impacts  precision.  Currently,  SA  is  in 
operation  on  all  Block  II  space  vehicles  which  comprise  the  majority  of  the  constel¬ 
lation.  The  DOD’s  stated  goal  for  the  positioning  accuracy  under  SA  is  100  meters 
(twice  rms)  for  a  two-dimensional  fix  [Ref.  15].  According  to  [Ref.  16,  p.  420], 
the  selective  availability  error  can  be  modeled  as  a  zero-mean,  five  meter  standard 
deviation  low  frequency  noise.  The  suggested  cutoff  frequency  for  SA  noise  is  t^Hz. 
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3.  Clock  Differences 


The  clock  model  used  in  this  treatment  of  DGPS  is  a  two  state  model 
shown  in  Figure  8.3.  It  is  reasonable  to  expect  the  clock  to  have  both  a  bias  and 


Figure  8.3:  Receiver  Clock  Model 


drift.  From  daily  exposure  to  clocks,  the  average  person  realizes  that  most  clocks 
are  slightly  offset  from  correct  time  (bias),  and  that  their  accuracy  tends  to  degrade 
with  time  (drift).  From  an  engineering  standpoint,  these  two  phenomena  can  be  best 
modeled  with  zero  mean,  white,  Gaussian  noise.  Rewriting  the  model  in  state  space 
form  for  further  analysis  yields 


The  covariance  of  the  clock  error  state  (sq)  can  be  found  by  solving  the  Lyapunov 
equation  which  can  be  found  numerous  control  textbooks,  one  of  which  is  [Ref.  17,  p. 
104].  This  equation  must  be  solved  over  a  finite  time  interval  (At)  because  the  two 
state  clock  model  is  unstable.  This  interval  would  normally  be  the  sampling  time,  if 
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the  model  were  discrete.  The  result  is 


£(*?)  =  SiAi  + 


(8.13) 


By  taking  the  square  root  of  the  variance  and  dividing  by  At,  one  finds  the  more 
standard  clock  parameter  the  Allan  variance,  VA 


VA  = 


(8.14) 


A  representative  plot  of  the  two  state  model  Allan  variance  as  a  function  of  averaging 
time  (i.  e.  At)  is  shown  in  Figure  8.4. 


Figure  8.4:  Ideal  Allan  Variance 

Real  clocks  behave  somewhat  differently  from  this  simple  model.  A  typical 
Allan  variance  plot  for  a  real  clock  is  shown  in  Figure  8.5. 
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Figure  8.5:  Real  Allan  Variance 


The  flat  portion  of  the  curve  is  called  the  flicker  floor.  It  is  the  result  of 
a  non-linear  effect  which  cannot  be  modeled  by  the  two  state  model.  This  causes  a 
significant  discrepancy  between  this  simple  model  and  the  real  world. 

In  order  for  the  model  to  better  represent  reality,  it  must  be  carefully 
crafted  to  fit  the  actual  plot  as  much  as  possible.  By  carefully  choosing  the  values  of 
Si  and  S2,  it  is  possible  to  make  the  actual  and  model  curves  fairly  close.  The  key 
Allan  variance  parameters  between  0.1  and  ten  seconds  of  averaging  time  are  ho ,  h- 1, 
and  A_2  [Ref.  18].  Values  of  these  three  parameters  for  three  common  GPS  timing 
standards  are  shown  in  the  Table  8.1  from  [Ref.  16,  p.  428]. 

Brown  [Ref.  16]  finds  that  one  must  choose  where  the  2-state  model  is 
accurate.  Since  normal  averaging  times  are  in  the  0.1  to  ten  second  interval  already 
mentioned,  this  is  the  region  where  the  model  is  made  accurate.  Maximizing  the 
accuracy  of  the  model  in  that  region  dictates  the  following  values  for  the  noise  spectral 
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TABLE  8.1:  ALLAN  VARIANCE  PARAMETERS  FOR  THREE  COM¬ 
MON  TIMING  STANDARDS 


timing  standard 

ho 

h-x 

h- 2 

crystal 

2  x  1CT19 

7  x  HT21 

2  x  lO"20 

ovenized  crystal 

8  x  10-2° 

2  x  10"21 

4  x  1(T23 

Rubidium 

2  x  HT20 

7  x  10~24 

4  x  10"29 

densities: 

ho 

rs_/  - 

2 

~  2 tt2/i_2  s"2.  (8.15) 

In  order  to  remain  conservative  in  this  generic  model  of  DGPS,  the  least 
accurate  clock  —  the  crystal  clock  —  is  used.  The  values  for  Si  and  S2  for  this  clock 
are 

Sx  =  4  x  1(T19 

S2  =  1.58  x  1(T18  s"2.  (8.16) 

These  values  are  be  used  in  the  DGPS  error  model. 

4.  Ephemeris  Error 

In  converting  the  pseudoranges  of  at  least  four  satellites  (six  in  this  model) 
to  a  three-dimensional  position  and  clock  error,  one  must  solve  a  series  of  non-linear, 
coupled  algebraic  equations.  In  these  equations,  the  positions  of  the  satellites  are 
critical.  The  only  way  a  GPS  receiver  or  navigation  filter  knows  the  space  vehicle 
positions  is  the  navigation  message.  If  broadcast  satellite  positions  are  incorrect, 
the  accuracy  of  the  resulting  receiver  position  suffers.  The  control  segment  of  the 
GPS  system  maintains  positions  on  the  entire  constellation  quite  accurately. However, 
it  would  be  folly  to  expect  the  satellites  to  broadcast  inerrantly  accurate  positions. 
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Typical  ephemeris  inaccuracies  according  to  [Ref.  19]  are  shown  in  Figure  8.6. 


These  errors  are  resolved  in  a  coordinate  system  local  to  each  space  vehi¬ 
cle.  The  three  mutually  perpendicular  directions  are  radial,  along  track,  and  cross 
track.  Because  the  DGPS  model  developed  in  this  thesis  does  not  account  for  satel¬ 
lite  motion,  along  track  and  cross  track  directions  cannot  be  resolved.  To  remain 
conservative,  these  two  errors  are  combined  into  a  circular  error  in  the  plane  they 
define.  This  error  is  modeled  as  zero  mean,  white,  Gaussian  noise  with  a  standard 
deviation  of  3.5  meters.  Likewise,  the  radial  error  is  modeled  as  zero  mean,  white 
Gaussian  noise  with  a  0.5  meter  standard  deviation.  Both  of  these  errors  make  the 
stated  accuracy  the  two  rms  point.  In  other  words,  the  value  stays  within  the  stated 
accuracy  95%  of  the  time. 

Since  the  space  vehicle  location  enters  the  model  in  earth  centered,  earth 
fixed  Cartesian  coordinates  while  the  error  is  added  in  geodetic  coordinates,  a  trans¬ 
formation  is  between  the  two  coordinate  systems  must  be  performed.  Having  con¬ 
verted  the  satellite  positions  to  geodetic  coordinates,  the  random  errors  can  be  added. 
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However,  since  the  geodetic  coordinates  contain  angles,  the  random  errors  in  position 
must  be  converted  to  equivalent  angles  in  latitude  and  longitude  by  dividing  by  the 
radius  of  the  space  vehicles’  orbits,  ~26,560,000  meters.  The  position  and  the  error 
are  now  in  compatible  coordinates  and  can  thus  be  summed. 

5.  Multipath 

The  signal  radiated  by  a  satellite  is  not  required  to  take  a  direct  path  to 
a  receiver.  If  the  signal  encounters  an  electromagnetically  reflective  object,  it  may 
bounce  off  that  object  and  still  find  its  way  to  the  receiver.  This  signal  has  now 
traveled  a  greater  distance  than  the  straight  line  joining  the  space  vehicle  and  the 
receiver.  Because  the  receiver  assumes  the  direct  path  is  used,  this  multipath  phe¬ 
nomenon  can  introduce  pseudorange  errors.  Due  to  the  satellite/receiver  geometry, 
multipath  is  far  more  likely  at  low  elevation  angles.  The  GPS  system  has  several 
attributes  that  minimize  multipath  effects  [Ref.  10]: 

•  The  L  band  frequency  (1227.6  MHz)  tends  to  undergo  diffuse  rather  than  spec¬ 
ular  reflection. 

•  The  receiver  antennas  tend  to  reject  multipath  signals. 

•  The  navigation  message  is  broadcast  with  circular  polarization.  Circularly  po¬ 
larized  signals  undergo  reversal  upon  reflection. 

•  GPS  receivers  generally  use  mask  angles  (the  elevation  angle  below  which  the 
satellite  is  ignored)  of  at  least  five  degrees. 

All  of  these  factors  tend  to  attenuate  the  strength  of  any  reflected  signal  making  the 
multipath  effect  insignificant.  Therefore,  it  is  not  modeled. 
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6.  Receiver  Noise 

All  receivers  corrupt  the  signals  they  receive.  GPS  receivers  are  no  excep¬ 
tion.  Inaccuracies  resulting  from  quantization  error,  loop  tracking  errors  and  other 
hardware  inadequacies  corrupt  the  pseudorange  accuracy.  According  to  [Ref.  10], 
representative  GPS  receiver  noise  has  a  standard  deviation  of  7.5  m.  Receiver  noise 
is  thus  modeled  as  a  zero  mean,  white  Gaussian  noise  with  7.5  m  standard  deviation. 
As  the  differential  station  and  the  aircraft  receivers  are  assumed  identical,  both  air¬ 
craft  pseudoranges  and  differential  station  pseudoranges  are  contaminated  with  this 
noise.  The  noise  in  the  two  receivers  is  assumed  to  be  independent. 

7.  Dilution  of  Precision 

All  of  the  errors  discussed  to  this  point  directly  effect  pseudorange  accu¬ 
racy.  The  various  delays  and  noise  sources  can  cause  the  receiver’s  evaluation  of  range 
to  the  space  vechicle’s  to  be  inaccurate.  However,  the  pseudoranges  themselves  are 
irrelevant.  The  science  of  navigation  is  concerned  with  positioning.  Dilution  of  Pre¬ 
cision  is  the  effect  that  links  pseudorange  accuracy  to  position  accuracy.  DOP  can 
be  further  classified  into  several  different  types: 

•  VDOP  —  Vertical  DOP  (z) 

•  HDOP  —  Horizontal  DOP  (x,y) 

•  PDOP  —  Position  DOP  (x,y,z) 

•  TDOP  —  Time  DOP  (t) 

•  GDOP  —  Geometric  DOP  (x,y,z,t) 

The  equation  which  relates  DOP  and  pseudorange  error  is 

CTj,  =  DOP  •  <70,  (8.17) 


119 


where  ap  is  the  standard  deviation  of  the  position  error  and  <7o  is  the  standard  devia¬ 
tion  of  the  pseudorange  error,  often  called  the  User  Equivalent  Range  Error  (UERE). 

Dilution  of  Precision  is  a  function  of  satellite  to  receiver  geometry.  As 
Figure  8.7  shows  for  a  four  satellite  constellation,  GDOP  is  minimized  with  the  space 


POOR  GDOP 
satellites  bunched 
together 


GOOD  GDOP 
(ideal  case) 

one  satellite  overhead 
3  on  horizon, 

120°  apart  in  azimuth 


Figure  8.7:  Dilution  of  Precision  from  [Ref.  5,  p.  4.22] 


vehicles  spread  out  as  much  as  possible.  In  fact,  the  volume  of  the  tetrahedron 
formed  by  the  unit  vectors  from  the  receiver  to  each  satellite  is  an  empirical  measure 
of  DOP.  PDOP  is  inversely  proportional  to  the  volume  of  the  tetrahedron.  If,  for 
example,  all  of  the  space  vehicles  a  receiver  was  using  for  positioning  were  in  the 
same  plane,  PDOP  would  approach  infinity  (the  volume  of  the  tetrahedron  would  be 
zero).  Likewise,  minimum  PDOP  is  achieved  with  the  geometry  shown  at  the  right 
of  Figure  8.7. 

The  goal  for  the  design  of  the  entire  GPS  constellation  of  satellites  is  a 
PDOP  no  greater  than  six  everywhere  on  the  earth.  With  the  entire  set  of  24  space 
vehicles  now  in  orbit,  users  can  expect  PDOP  values  under  three  [Ref.  20]. 
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The  entire  concept  of  GPS  navigation  has  now  been  thoroughly  discussed. 
All  of  the  information  put  forth  in  this  discussion  will  be  used  in  developing  the  DGPS 
computer  model  in  the  next  chapter.  To  complete  the  sensor  discussion,  a  description 
of  INS  follows. 

F.  INERTIAL  NAVIGATION 

Inertial  navigation  has  long  been  the  standard  for  self-contained,  long-range 
aircraft  navigation.  An  Inertial  Navigation  System  (INS)  senses  aircraft  thrust  accel¬ 
eration,  angular  rates  and  spatial  orientation  resolved  in  an  orthogonal  system  and 
computes  the  inertial  acceleration.  This  acceleration  can  now  be  integrated  —  once 
to  find  velocity,  and  twice  to  find  position. 

The  primary  component  in  any  inertial  navigation  system  is  the  Inertial  Measur¬ 
ing  Unit  (IMU).  The  IMU  is  composed  of  three  accelerometers,  three  rate  gyros,  and 
two  inclinometers.  The  accelerometers  measure  thrust  acceleration.  Thrust  accelera¬ 
tion  is  composed  of  linear,  centripetal,  and  gravitational  effects.  Einstein’s  Principle 
states  that  it  is  impossible  for  a  sensor  to  distinguish  between  the  effects  of  gravity 
and  acceleration.  Thus,  the  thrust  acceleration  that  it  provides  is 

ba  =bv  +  buxbv  +bg,  (8.18) 

where  ba  is  the  thrust  acceleration,  bv  is  the  linear  acceleration,  bu>  is  the  angular 
velocity,  bv  is  the  velocity,  and  bg  is  gravity.  All  quantities  are  in  the  body  frame.  This 
principle  necessitates  that  the  computation  portion  of  the  INS  compute  and  remove 
local  gravity  from  the  measured  accelerations. 

Rate  gyros  sense  angular  velocities.  These  angular  velocities  can  be  resolved 
in  either  the  body  or  inertial  frame,  depending  on  the  type  of  IMU  implementation. 
The  two  inclinometers  sense  aircraft  inertial  orientation,  i.e.,  Euler  angles. 
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There  are  two  conceptual  methods  for  implementing  inertial  navigation: 


•  Gimbaled  IMU 

•  Strapdown  IMU 

Brief  discussions  of  each  method  follow. 

1.  Gimbaled  IMU 

A  gimbaled  IMU  rotates  about  its  four  gimbals  during  operation  (see  Fig¬ 
ure  8.8).  Aircraft  IMU’s  must  have  four  gimbals  to  prevent  gimbal  lock,  while  earth- 

Outer 


Figure  8.8:  Gimbaled  IMU  from  [Ref.  15,  p.  193] 

bound  IMU’s  require  only  three  gimbals  [Ref.  21,  p.  192].  A  controller  maintains 
the  IMU  in  a  constant  inertial  orientation  toward  true  North  and  lying  in  the  locally 
horizontal  plane.  By  maintaining  this  orientation,  the  gimbaled  IMU  measures  iner¬ 
tial  quantities  directly.  This  data  can  be  integrated  without  transformation  to  yield 
inertial  velocity,  position  and  Euler  angles.  From  a  navigation  standpoint,  this  con¬ 
figuration  seems  ideal.  However,  gimbaled  systems  are  large  and  heavy,  making  them 
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impractical  for  small  aircraft.  Also,  kinematic  quantities  resolved  in  the  aircraft -fixed 
coordinate  system,  necessary  for  stability  augmentation  and  control,  are  not  directly 
available.  Instead,  they  must  be  computed  through  a  series  of  Euler  rotations.  The 
computations  required  take  time  and  thus  introduce  delays  into  an  often  time  critical 
control  problem.  This  fact  makes  the  gimbaled  system  less  than  desirable  for  control. 

2.  Strapdown  IMU 

Strapdown  IMU  is  conceptually  the  reverse  of  the  gimbaled  system  men¬ 
tioned  above.  Rather  than  maintaining  a  constant  inertial  orientation,  a  strapdown 
system  is  “strapped  down”  to  the  aircraft,  thus  maintaining  a  constant  orientation 
in  the  aircraft-fixed  coordinate  system.  Therefore,  the  output  of  the  IMU  is  resolved 
in  the  local  coordinate  system.  Kinematic  quantities  are  immediately  available  for 
the  control  system.  The  extra  computational  burden  now  rests  on  the  navigation 
computer  which  must  transform  the  accelerations  and  angular  velocities  sensed  in 
the  local  coordinate  system  to  the  inertial  system.  Currently,  most  inertial  systems 
are  of  the  strapdown  variety.  With  the  advent  of  high  speed,  low  cost,  lightweight 
computing  power,  the  required  calculations  in  transforming  from  the  aircraft-fixed 
to  the  inertial  coordinate  system  are  no  obstacle  to  navigation.  This  is  the  system 
which  is  modeled  in  this  thesis. 

G.  INS  COMPUTATIONS 

In  the  strapdown  configuration,  the  IMU  measures  angular  velocities  and  thrust 
acceleration  in  the  body  frame,  as  well  as  Euler  angles.  However,  the  INS  must  provide 
position  and  orientation,  both  in  the  inertial  frame.  In  order  to  compute  position  and 
orientation  in  the  inertial,  tangent  plane  coordinate  system,  inertial  accelerations  and 
Euler  rates  must  be  calculated.  Before  converting  the  inertial  acceleration  from  the 
body  frame  to  the  inertial  frame,  it  is  necessary  to  compute  the  inertial  orientation. 
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Computing  the  Euler  rates  is  a  tricky  endeavor.  The  Euler  rates  are  simply  related 
to  the  body  angular  rates  by  Poisson’s  equation 
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where  p.  q,  and  r  are  the  components  of  bu>,  the  body’s  angular  velocity.  Obviously, 
this  formula  requires  the  exact  Euler  angles.  However,  one  can  only  measure  these 
angles  directly  at  very  low  frequency.  At  frequencies  greater  than  a  fraction  of  a  Hertz, 
one  must  integrate  the  Euler  rates  found  from  Equation  8.19  to  find  the  angles.  This 
presents  the  seeming  paradox  of  needing  to  know  the  Euler  angles  in  order  to  find 
the  Euler  rates  which  must  be  integrated  to  find  the  Euler  angles.  This  process 
must  be  implemented  recursively.  That  is,  the  results  of  the  integration  to  find  the 
angles  must  be  fed  back  into  Equation  8.19.  Furthermore,  a  complementary  Kalman 
filter  is  necessary  to  provide  an  optimal  estimate  of  the  Euler  angles,  trusting  the 
inclinometers  at  the  low  frequencies  and  the  rate  gyros  at  high  frequencies. 

Now  having  inertial  acceleration  in  expressed  in  the  body  frame  and  the  orien¬ 
tation  of  the  body  (Euler  angles),  the  transformation  from  body  to  inertial  can  be 
executed.  This  coordinate  transformation  is  defined  by 

uP=uhCba  +  ug ,  (8.20) 

where  $,  0,  and  T  are  the  roll,  pitch  and  yaw  Euler  angles,  respectively,  UP  is  the 
aircraft’s  acceleration  in  inertial  coordinates,  ba  is  thrust  acceleration  in  the  aircraft- 
fixed  coordinate  system,  ug  is  gravity  in  inertial  coordinates  and  \C  is  the  transfor¬ 
mation  matrix  from  the  body-fixed  coordinates  to  inertial  tangent  plane  coordinates 
as  follows 

cos  $  cos  0  cos  $  sin  0  sin  $  -  sin  $  cos  $  cos  if  sin  0  cos  $  +  sin  >5  sin  $ 
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It  is  UP  which  can  be  integrated  to  provide  velocity  and  position  in  the  inertial, 
tangent  plane  frame. 

H.  INS  ERROR  SOURCES 

The  Inertial  Measuring  Unit  is  subject  to  a  few  main  error  sources.  These  are: 

•  bias 

•  cross-axis  sensitivity 

•  noise  floor 

All  of  these  errors  apply  to  both  accelerometers  and  rate  gyros.  Brief  descriptions  of 
these  problems  follow. 

1.  Biases 

A  bias  in  an  accelerometer  or  a  rate  gyro  is  defined  as  a  constant  offset  of 
the  output  of  the  device  from  the  true  value.  In  other  words,  an  accelerometer  might 
constantly  read  0.01  m/s2  while  the  device  is  not  accelerating.  This  0.01  m/s2  would 
be  referred  to  as  a  bias.  This  error  is  modeled  with  a  series  of  small  step  functions, 
one  for  each  accelerometer  and  rate  gyro. 

2.  Cross— Axis  Sensitivity 

Cross-axis  errors  are  caused  by  misalignment  of  the  IMU  with  the  aircraft 
coordinate  axes.  Ideally,  the  components  of  the  IMU  —  the  accelerometers  and  the 
rate  gyros  —  would  each  be  perfectly  aligned  with  the  three  axes  of  the  aircraft-fixed 
coordinate  system.  Unfortunately,  even  the  highest  fidelity  inertial  sensors  are  never 
precisely  coincident  with  the  appropriate  axes.  Misalignment  of  both  types  of  devices 
causes  errors.  The  cross-axis  errors  are  modeled  with  the  following  equation: 

0  eY  ez  ' 

aca  =  ex  0  ez  a,  (8.21) 

ex  ey  0 
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where  aca  is  the  cross-axis  error,  ex,  eY,  and  ez  are  the  cross-axis  error  terms  and 
a  is  the  actual  acceleration  resolved  in  the  aircraft  coordinate  system.  The  e’s  from 
the  previous  equation  are  determined  by  the  amount  of  angular  offset  of  each  sensor 
from  the  correct  position. 

3.  Noise  Floor 

All  sensor  devices  corrupt  the  quantities  they  measure.  Various  factors 
including  thermal  noise  can  cause  a  constant  output  of  white  noise,  regardless  of  the 
actual  acceleration  (or  angular  velocity).  This  noise  floor  makes  accelerations  below  it 
not  measurable.  That  is,  the  output  of  the  sensor  still  includes  the  actual  quantities, 
but  it  is  “invisible”  because  the  noise  floor  obscures  it.  This  process  is  modeled  by  in¬ 
troducing  a  threshold  to  the  actual  acceleration  in  the  aircraft  coordinate  system  and 
adding  white  noise.  Taking  these  steps  result  in  the  accelerometer  always  reporting 
noisy,  zero-mean  acceleration  unless  the  actual  value  is  above  the  threshold.  When 
the  actual  value  exceeds  the  threshold,  that  value  is  added  to  the  noise  floor  value 
(and  the  bias)  to  create  the  output  of  the  accelerometer.  This  process  also  applies  to 
the  rate  gyros.  Now  both  of  the  sensors  —  GPS  and  INS  —  have  been  completely 
investigated. 
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IX.  KALMAN  FILTER  DESIGN 


First,  recall  the  general  process  for  filter  design: 

1.  Obtain  navigation  equations: 

x  =  f(x,u) 

y  =  h(x,u)  (9.1) 

2.  Linearize  9.1  at  (x0,u0): 

8x  =  ASx  +  B8u 

6y  —  C8x-\-D8u  (9-2) 

3.  Now,  we  need  to  use  9.2  to  design  a  filter: 


i)  Use  9.2  to  create  a  synthesis  model: 


8xs  —  As8xs  +  Bis8w  +  B2S8u 
8y s  —  AJs8x^  A  8v 


where 
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8xt  are  the  states  resulting  from  appended  integrators. 
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(9.4) 


Figure  9.1:  Synthesis  Model 


ii)  Let  W  =  I  and  V  =  I  (intensity)  where: 

E  (Sw(t)Sw(t)T)  =  W8(t  —  t) 

E  (6v(t)8v(t)T'j  =  VS(t  —  T ) 

and  solve  the  Ricatti  equation  for  the  Kalman  gain: 

+  XAts  -  XCjV-'CsX  +  BlsWBl  =  0 

Hs  =  XCjV'1  =  [H  Hr] 

iii)  Form  the  filter: 

j  Sx  =  AsSxs  +  Hs  (Sy  -  6y) 

\  Sy  =  CSx,  <9'5) 

iv)  Now  compute  the  transfer  function  from  6y  to  Sy. 

v)  If  the  bandwidth  is  not  sufficient  to  meet  the  design  criteria,  go  back  to 
step  (ii)  where  a  new  value  of  V  is  chosen. 


4.  Now  implement  the  filter  on  the  non-linear  plant: 


=  f{x„u)  +  H,(y-y) 
y  lx  (3:55  u) 


(9.6) 
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Figure  9.3:  Tangent  Plane  Mechanization 
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Figure  9.4:  Body  Frame  and  Tangent  Plane  Relationship 

Now  let’s  apply  this  process  to  Kalman  filter  design  for  tangent  plane  navigation. 
1.  Obtain  navigation  equations:  Let 
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Figure  9.5:  Pseudorange 


9(P)  II  Dll?  1 
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Therefore,  using  our  notation  we  get 


uA  +  g(P)  = 


where  v0b  =  uP0b-  Note  9.7  is  in  the  form 


=  /(x,  u ),  where  x  = 


and  u  =  u A 


Now,  we  need  to  include  the  pseudorange  equations 


Pi  —  e  P  —  e  P si  d"  cAf 


=t  a. 


where  cAt  =  UERE  (User  Equivalent  Range  Error). 

Note,  all  computations  here  must  be  done  in  {e},  since  satellite  positions  are 
given  in  ECEF. 

Let  euR  be  given:  Computing  euR  can  be  done  by  selecting  the  latitude  and 
longitude  for  the  origin  of  {«}.  Then: 

UP  =  euRuP0b+  eP0u 

where 

0 

ePo»  =  euR  0 

.  Ro . 

Now, 

P*  =  II  euR  uPob  +  ePou  -  ePsx ||  +  CAt  (9.9) 

Finally,  we  need  to  compute  At.  Since  the  only  real  uncertainty  in  UERE  is 
receiver  clock  error,  we  get: 

w: . 

bias 
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Figure  9.6:  User  Equivalent  Range  Error  Mechanization 


or 


re  =  Ad  +  Wbias 
Ad  =  W drift 


(9.10) 
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s(  UP)  = 


where 


V^2  +  i?o)2j 
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z  +  jR0 
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)  It  can  be  shown  that  given  y  =  ||i?P||,  where  R~l  —  RT 


dPV  \\RP\\RTRP 


Using  9.14  we  get 


aftAi  _  »a.-  'Pjf 


*  RUP  J -  ep  _  e  p 

U±L  1  06  '  1  Ou  r  sx 


(Pq+  “ig(eP0u-  eP„)f 
I \euRuPo+  ePou  -  ePsx II 

Collecting  terms,  we  obtain  the  linear  error  equations: 
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0  0 

1  1 

°  0  JM 

o  4$o 
i 

03x3 

0  0 

0l,3 

0l,3 

Or, 3 

0l,3 

0  1 
0  0 

(9.14) 
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Note,  Equations  9.15  and  9.16  are  in  the  form: 


r  -  ' 

<  6x  =  A6x  +  B6u+  j  6w 
k  6y  =  CSx 


3.  Use  9.17  to  design  a  Kalman  filter. 


(9.16) 


(9.17) 
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X.  GPS/IMU  INTEGRATION 


Now  that  the  kinematic  equations  and  coordinate  systems  have  been  discussed 
in  detail,  the  next  step  is  to  design  the  navigation  system  using  MATLAB  /  Simulink 
software  development  tools.  The  aircraft  chosen  to  evaluate  the  design,  was  the 
Airplane  D,  a  medium  size,  high  performance  business  like  jet  from  Roskam  [Ref.  4, 
Appendix  C].  Mission  requirements  for  the  design  were  consistent  with  a  medium- 
to-high  altitude  reconnaissance  aircraft  cruising  in  straight  and  level  flight,  taking 
high  resolution  photographs  of  various  military  and  industrial  sites.  Flight  condition 
2  from  [Ref.  4,  Table  C4]  specifies  a  maximum  weight  of  (13,000  lbs.),  high  altitude 
(40,000  ft)  cruise,  at  677  feet  per  second  (M=0.7).  At  this  condition,  the  aircraft  has 
a  pitch  angle  of  2.7  degrees  (0.047  radians),  a  trim  lift  coefficient  of  0.41  and  a  trim 
drag  coefficient  of  0.0335.  Additional  aircraft  data  includes  a  wing  area  of  230  square 
feet,  a  wing  span  of  234  feet,  a  mean  geometric  chord  of  7  feet  and  a  center  of  gravity 
(c.g.)  at  0.32  mean  aerodynamic  chord  (MAC).  The  stability  and  control  derivatives 
used  are  presented  in  Roskam  [Ref.  4,  p.  620] 

A.  EQUATIONS  OF  MOTION 

The  first  step  in  the  design  of  the  integrated  system  is  to  develop  the  appropriate 
equations  of  motion  (EOM)  for  the  aircraft.  A  right-handed  body  coordinate  system 
{6}  was  chosen  referenced  to  a  universal  frame  {u}  with  a  NED  orientation.  The 
derivation  of  the  EOM’s  starts  with  the  following  four  equations: 

ri  6  7A 

-Vb  =  -bubx  bVb  +  — 

at  m 

=  —Ib  1  ^  bUb  X  ^Ib  +  Ib  1  b N 

A  =  5(A)  biob 
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uPb  =  “ Cbvb 

where 

F  =  Forces 

N  =  Moments 

The  force  and  moment  terms  consist  of  the  thrust,  aerodynamic  and  gravity 
terms.  Assuming  the  moment  about  the  c.g.  due  to  gravity  is  zero,  the  force  and 
moment  equations  reduce  to: 

F  =  Fj  +  Fa  +  Fg 

N  =  Nt  +  Na 

where 

T  =  Thrust 
A  =  Aerodynamic 
g  =  Gravity 

The  thrust  terms  are  computed  from  the  engine  thrust  settings  and  aircraft  geome¬ 
try  and  the  gravitational  force  is  computed  by  resolving  the  gravitational  constant 
32.174^3  from  the  universal  to  the  body  frame. 

The  aerodynamic  forces  are  calculated  using  a  first  order  approximation  of  the 
stability  and  control  derivatives  contained  in  Roskam  [Ref.  4].  Starting  with  funda¬ 
mental  aerodynamic  force  equation: 

F  =  i  pV2SCF  (10.1) 

where 

F  =  [-D,-Y,-L,l,m,n]  (10.2) 
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the  first  order  linear  approximation  becomes: 

F  =  *S  {C)1  +  £  {Cf)  (x'  “  *'») +  li'  ^  <*'  -^  +  7K  {Cf)  (a  “  A»>) 

where 

T  1T 

x  —  u  v  w  p  q  r 

x!  —  —  u  v  w  &  &  br  } 

Vt  L  i  i  \ 

x'  =  v!  /?'  a1  p'  q'  r' 

S  =  diag  [  -S  S  -S  Sb  Sc  Sb  ' 

A  =  8e  8a  8r  ST  ] 

The  initial  development  of  the  EOM  involves  only  the  jtbVb  and  jbub  equations. 
In  matrix  form  this  becomes  the  basic  EOM  equation: 


’  Ttbyb  1  =  [  x  ^  If  buCug  ' 

_Ttb“b  J  [  -I,1  (  bub  ( huh  x  (lb  x  (4  W))))  J  +  [  0 


where  Ib  is  the  aircraft’s  inertia  tensor  and  the  aerodynamic  forces  and  moments 
are  computed  in  the  wind  reference  frame  {w}  and  must  be  converted  to  the  body 
frame  {&}.  The  first  order  approximation  of  the  aerodynamic  forces  and  moments  was 
substituted  into  the  basic  EOM  equation  and  the  A  equation  was  added  yielding  the 
complete  EOM  equation.  After  the  trim  values  were  computed,  they  were  included 
as  initial  conditions  in  the  EOM’s  integrator.  The  diagram  was  then  linearized, 
using  the  MATLAB  linmod.m  command.  EOM  verification  was  done  by  comparing 
the  eigenvalues  of  the  linearized  model  to  Roskam’s  data.  The  results  are  shown  in 
Table  10.1. 
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TABLE  10.1:  EIGENVALUES 


Roskam 

EOM 

Long  period 

-0.00593  ±  0.0904i 

-0.0103  ±  0.0912; 

Short  period 

-0.9943  ±  2.641z 

-0.7941  ±  2.714; 

Spiral 

-0.0012 

+0.0007 

Dutch  roll 

-0.05848  ±  1.684i 

-0.0567  +  1.686; 

Roll  mode 

-0.5023 

-0.5036 

The  linearized  values  compare  quite  nicely  to  Roskam's  values  except  for  one 
unstable  eigenvalue  in  the  lateral  dynamics.  This  pole  corresponds  to  the  spiral  mode 
and  has  a  large  time  constant.  It  does  not  affect  stability  in  the  longitudinal  dynamics. 
Finally,  the  three  position  states  were  added  to  the  initial  EOM  model  to  make  the 
final  EOM  model,  shown  in  Figure  10.1,  which  is  used  as  an  input  to  the  IMU  System. 


Figure  10.1:  Equations  of  Motion  Simulink  Block  Diagram 


140 


B.  INERTIAL  MEASUREMENT  UNIT  (IMU) 

In  order  for  the  IMU  to  calculate  position  and  velocity  in  the  universal  frame  u, 
the  aircraft’s  angular  rates  about  the  x,  y ,  and  z  axes  (p.  9,  and  r  respectively)  and 
thrust  accelerations  in  ax,  ay,  and  az,  must  be  measured.  A  real  IMU  uses  angular 
gyros  and  accelerometers  to  provide  this  data,  where  the  design  IMU  model,  Fig¬ 
ure  10.2,  uses  state  vector  outputs  from  the  EOM  model  to  perform  the  acceleration 


IMU  model  (Level  1) 


Figure  10.2:  Inertial  Measurement  Unit  (Level  1)  Simulink  Block  Diagram 

calculations.  The  IMU  inputs  consist  of  the  linear  velocity  components  («,  v,  w)  or 
Vb  ,  angular  rates  (p,  q ,  r)  or  u>b  ,  Euler  angles  (<f>,  0,  t/;)  or  A  ,  and  the  position  of  the 
body  in  the  universal  frame,  uPb.  Additionally,  the  linear  accelerations,  Vb,  are  also 
provided  to  complete  the  calculations.  ’Given  the  small  size  of  the  aircraft,  it  was 
assumed  that  the  IMU  was  located  at  the  aircraft’s  center  of  gravity  and  all  calcu¬ 
lations  are  based  on  this  assumption.  Since  real  IMU  sensors  possess  some  inherent 
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noise  and  bias  characteristics,  white  gaussian  noise  was  included  to  simulate  the  noisy 
sensors  and  a  nominal  bias  to  account  for  the  design  limitations  of  the  sensors. 

To  calculate  the  accelerations  in  the  {u}  frame,  Figure  10.3,  the  gravity  force 


IMU  mode!  {Level  2) 


Figure  10.3:  Inertial  Measurement  Unit  (Level  2)  Simulink  Block  Diagram 

was  calculated  and  a  transformation  matrix  from  {u}  to  the  body  frame  {b}  was 
applied: 

bg=buCug  (10.3) 

The  accelerations  in  the  body  frame  where  calculated  and  the  acceleration  due 
to  gravity  was  removed: 

ba  =  jt  bVb  +  bub  x  bVb  -  bg  (10.4) 

The  MATLAB  routine  to  perform  these  calculations  was  accel.m  and  is  provided 
in  Appendix  B.  After  the  accelerations  in  the  body  frame  were  calculated,  the  noise 
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and  bias  errors  were  incorporated  to  simulate  actual  acceleration  measurements  from 
the  IMU.  Since  they  were  calculated  in  the  body  frame,  the  noisy  accelerations,  ban, 
were  transformed  to  the  universal  frame  using  a  transformation  matrix,  %C.  The 
MATLAB  function  used  to  perform  this  calculation  was  abn2un.m.  The  resulting 
outputs  from  the  IMU  were  noisy  accelerations  in  the  universal  frame,  uan. 

C.  GLOBAL  POSITIONING  SYSTEM  (GPS) 

In  order  to  provide  accurate  position  information  that  could  be  used  to  up¬ 
date  or  verify  the  position  estimates  calculated  from  the  IMU  accelerations,  a  GPS 
mechanization  was  designed.  The  GPS  mechanization,  Figure  10.4,  was  designed  to 


GPS  model 


Biasl 


Figure  10.4:  GPS  Pseudo— range  Simulink  Block  Diagram 

provide  a  pseudo-range  measurement  based  on  the  aircraft’s  estimated  position  and 
position  of  four  satellites.  Inputs  to  the  GPS  came  from  the  EOM  linear  veloc¬ 
ity  terms,  Vb  ,  which  were  transformed  from  the  body  frame  to  the  universal  frame 
and  integrated  to  provide  a  position  estimate,  u .  Satellite  positions  were  input 
as  a  constant  value  using  a  satellite  initial  position  MATLAB  function,  initsat.m.  A 
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MATLAB  function,  exact  .m,  calculated  the  exact  ranges  from  each  satellite.  User 
equivalent  range  error  (UERE)  was  determined  by  introducing  white  gaussian  noise 
to  simulate  the  GPS  receivers  inherent  noise  bias  and  clock  drift.  Combining  the 
exact  range  with  the  UERE,  a  pseudo  range  value  was  calculated.  Along  with  the 
noisy  accelerations  calculated  in  the  IMU,  the  pseudo— range  values  were  input  to  the 
Kalman  filter. 

D.  KALMAN  FILTER 

The  Kalman  filter  was  designed  for  the  purpose  of  providing  optimal  inertial 
velocities  and  positions  given  noisy  accelerations  from  the  IMU  and  pseudo-ranges 
from  a  GPS  receiver.  To  accomplish  this,  it  was  desired  to  have  a  break  frequency  of 
as  the  cutoff  below  which  only  GPS  information  would  be  valid.  This  constituted 
steady-state,  straight  and  level  flight.  For  a  maneuvering  aircraft,  frequencies  greater 
than  l  ~,  the  sampling  rate  of  the  GPS  would  not  be  capable  of  providing  adequately 
up-dated  position  information.  At  this  point,  the  accelerometer  data  would  be  con¬ 
sidered  valid.  A  design  criterion  of  1  ^  as  a  -3dB  cutoff  for  the  frequency  response 
was  chosen  to  meet  these  requirements. 

1.  Navigation  Model 

The  first  step  in  the  filter  design  was  to  develop  a  navigation  model  which 
provided  velocity  and  position  as  outputs.  Using  the  position  outputs,  pseudo-ranges 
could  be  calculated.  The  navigation  model  was  divided  into  two  components.  The 
main  component,  Figure  10.5,  received  the  noisy  accelerations  from  the  IMU  which 
were  then  integrated  twice  to  produce  velocity  and  position  in  the  universal  frame. 
The  second  component,  Figure  10.6,  calculated  estimated  pseudo-ranges  to  the  four 
satellites  given  their  position,  the  aircraft’s  estimated  inertial  position  based  on  the 
accelerations,  and  a  clock  drift  error  component.  The  output  of  the  navigation  model 
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Exact  range 


Figure  10.5:  Navigation  Model  Simulink  Block  Diagram 


Exact  Range 


Noise2 


Figure  10.6:  Kalman  Filter,  Pseudo-range  Simulink  Block  Diagram 

was  four  estimated  pseudo-ranges.  These  estimated  ranges  were  compared  to  actual 
pseudo-ranges  from  the  GPS  receiver. 
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2.  Linearized  Model 

The  next  step  was  to  linearize  the  navigation  model  at  a  nominal  aircraft 
position.  This  was  accomplished  by  using  the  MATLAB  function,  linmod.m.  on  the 
navigation  model.  The  linearized  state  space  matrices,  [A,  B,  C,  D],  were  then  used  in 
the  development  of  the  synthesis  model.  In  state  space  form,  the  linearized  equations 
become: 

8x  —  ASx  +  BSu 
8y  =  C6x  +  DSu 

Here  D  =  0. 

3.  Synthesis  Model 

The  next  step  in  the  design  of  the  Kalman  filter,  was  to  create  a  sythesis 
model  which  accounted  for  measurement  and  sensor  noise  inputs.  In  this  case  only 
sensor  noise  bias  was  considered.  By  considering  only  noise  bias  in  each  accelerometer 
axis,  only  one  integrator  per  axis  is  required.  These  appended  integrators  result  in 
three  poles  at  the  origin.  As  the  gains  are  increased,  the  poles  will  diverge  in  three 
directions.  One  on  the  negative  real  axis  and  the  other  two  will  depart  at  45  degree 
angles  out  the  left— half  plane.  To  insure  system  stability,  three  zeros  are  included 
so  that  the  poles  converge  to  a  value  along  the  negative  real  axis.  In  the  synthesis 
model,  zeroes  were  chosen  at  -I,  which  forces  the  poles  to  converge  on  the  negative 
real  axis.  In  addition  to  the  noisy  inputs,  the  accelerations  in  each  axis  are  included 
as  inputs.  The  resulting  synthesis  model  is  shown  in  Figure  10.7.  Next,  the  synthesis 
model  is  linearized  to  determine  the  synthesis  state  space  matrices,  [AS,BS,CS,DS\. 
The  resulting  state  space  form  is: 

8x$  —  As8xs  T  B\s8w  T  B2s8u 

8y  =  Cs8x-i~8v  (10.5) 
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Mux 


Figure  10.7:  Synthesis  Model  for  Kalman  Filter,  Simulink  Block  Diagram 

In  the  equation  10.5,  8v  is  zero  since  only  input  noise  is  considered. 

4.  Noise  Intensity  and  the  Non-Linear  Plant 

In  order  to  solve  the  Ricatti  equation  10.6  for  the  optimal  gain 

+  XATS  -  XCjV-'CsX  +  BuWBl  =  0  (10.6) 

the  noise  intensities  V  and  W  must  be  adjusted  to  provide  the  optimum  frequency 
response.  The  initial  values  were  chosen  to  be  identity  matrices,  I.  Once  the  val¬ 
ues  were  chosen,  the  MATLAB  function,  Iqe.m,  was  used  to  determine  the  optimal 
Kalman  gain,  Hs.  The  Kalman  gain  Hs  was  partitioned  based  on  the  arrangement  of 
the  states  in  the  Bs  matrix.  The  partitioned  Kalman  gains  were  implemented  on  the 
non-linear  plant  shown  in  Figure  10.8 
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Hbias 


Figure  10.8:  Non-Linear  Kalman  Filter 
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Next,  a  bode  plot  of  the  frequency  response  was  generated  and  the  band¬ 
width’s  were  observed.  The  process  was  repeated  using  a  function  called,  kal.m.  This 
allowed  the  weighting  factors  to  be  adjusted  until  the  optimum  frequency  response 
was  obtained.  It  was  necessary  to  decrease  the  weighting  factor  on  the  outputs  V  and 
increase  the  weighting  on  the  inputs  W.  The  optimal  bandwidth  of  1  —  was  acheived 
at  V  and  W  values  of  0.05 1  and  61,  respectively.  The  frequency  responses  from  the 
pseudo  range  to  the  pseudo  range  estimates  are  shown  in  Figures  10.9  and  10.10.  It 


Figure  10.9:  Frequency  Response,  Pseudo-range  to  Pseudo-range  Estimate 
1  &  2 

can  be  seen  that  the  -3dB  cutoff  occurs  at  exactly  1  ~  for  each  input.  The  frequency 
responses  of  the  filter  to  accelerations  in  the  x,  y  and  z  directions  are  shown  in  Fig¬ 
ure  10.11.  The  20dB  per  decade  slope  of  the  curve  below  the  frequency  of  0.1 
indicates  the  accelerometer  inputs  are  washed  out  at  low  frequencies.  This  is  within 
the  design  criteria.  To  verify  the  response  of  the  filter  to  a  disturbance  in  accelera¬ 
tion,  a  step  input  was  performed  in  each  channel  and  the  pseudo-range  responses  are 
shown  in  Figure  10.12. 
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Freq,  (rad/sec) 


Figure  10.10:  Frequency  Response,  Pseudo-range  to  Pseudo-range  Esti 
mate  3  &:  4 
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Figure  10.11:  Frequency  Response,  Accelerations  to  Pseudo-range  Esti 
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XI.  CONCLUSION 


The  purpose  of  this  thesis  has  been  to  provide  the  engineer  with  a  solid  physical 
and  mathematical  understanding  of  the  relationship  between  the  GPS/IMU  sensors 
and  the  air  vehicle  and  how  to  integrate  the  two  systems  properly. 

The  extensive  use  of  3-D  graphics  for  depicting  the  coordinate  relationships  and 
the  corresponding  transformations,  gives  meaning  to  the  physical  and  mathematical 
relationships  between  the  two  systems.  By  introducing  the  fundamental  concepts 
behind  inertial  sensors  and  global  positioning  systems,  the  data  each  provides  can 
be  understood  and  integrated  properly.  Next,  the  errors  that  are  common  to  both 
systems  are  discussed  and  error  analysis  is  applied  to  reduce  the  effects  on  the  output 
of  the  integrated  system.  Introducing  the  design  method  for  Kalman  filtering  allows 
the  engineer  to  develop  a  system  which  accounts  for  the  inherent  errors  and  guarantees 
positional  accuracy  no  matter  which  sensor  is  providing  the  information.  Developing 
the  design  through  the  use  of  M.A'ThAB  / Simulink  software,  allows  the  engineering 
team  the  opportunity  to  apply  all  of  the  concepts  introduced  in  this  thesis  and  gives 
them  the  flexibility  to  expand  on  the  basic  concepts. 

This  thesis  has  provided  the  uniform  framework  required  in  the  design  of  an  in¬ 
tegrated  GPS  /  IMU  system  using  Kalman  Filtering.  As  GPS  and  IMU  components 
become  smaller  and  more  lightweight,  their  use  in  Unmanned  Autonomous  Vehicle’s 
(UAV’s)  will  increase  dramatically.  The  need  for  precise  navigational  data  will  con¬ 
tinue  to  rise  as  the  requirements  for  UAV’s  increase  to  meet  their  predicted  threat. 
Providing  the  engineer  with  a  complete  document  which  approaches  the  design  pro¬ 
cess  in  a  step-by-step  manner,  is  essential  if  design  team  is  to  compete  effectively. 
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APPENDIX  A:  SHOWCASE,  SNAPSHOT, 
AND  XV  UNIX  SOFTWARE 


The  graphical  models  used  in  the  development  of  this  thesis  were  produced 
using  the  Silicon  Graphics  Indigo  computers  available  in  the  Aeronautical  Engineering 
Computer  Laboratory  and  the  Avionics  Design  Laboratory,  both  located  in  Halligan 
Hall.  The  software  utilized  to  produce  the  3-D  models  was  IRIS  Showcase  (version 
3.3). 

A.  IRIS  SHOWCASE  3.3 

To  access  the  Showcase  graphical  program,  type  “showcase”  at  the  prompt  in 
the  winterm  window.  Three  windows  will  appear  when  “Showcase”  comes  on-line. 
The  first  and  largest  window  will  be  the  drawing  tablet,  figure  A.l.  This  window 


Figure  A.l:  Showcase  Drawing  Tablet 


is  used  for  producing  the  actual  picture.  The  next  window  is  called  the  “Showcase 
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Master  Gizmo”.  This  is  the  main  tool  for  selecting  1-D  drawing  devices,  figure  A. 2. 
The  third  window  is  a  “status  gizmo”,  figure  A. 3,  which  displays  the  status  of  each 


Figure  A. 2:  Showcase  Master  Gizmo 

command  selected  or  action  performed  in  “Showcase” . 


Figure  A. 3:  Showcase  Status  Gizmo 

1.  Creating  1-D  and  3-D  Objects 

To  draw  1-D  figures,  select  the  appropriate  tool  from  the  “Master  Gizmo” 
using  the  mouse  “point  and  click”  method.  The  method  is  similar  to  all  graphics 
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programs  used  on  Macintosh  and  IBM  computers.  Type  styles  can  be  selected  from 
the  same  tool-box  and  fonts  can  be  chosen  at  the  lower  portion  of  the  “Master  Gizmo” 
window.  These  operations  are  self-explanatory. 

To  draw  3-D  objects,  the  strength  of  this  program,  go  to  the  “Gizmo”  pull 
down  menu  at  the  top  of  the  drawing  tablet.  Select  the  “3-D”  option  from  the  menu 
and  “3D  Gizmo”,  figure  A. 4,  will  appear.  Using  the  mouse  arrow,  select  the  “Create” 


Figure  A. 4:  Showcase  3D  Gizmo 

tool  in  the  upper  right  corner  of  the  “3D  Gizmo”  window.  Next,  place  the  mouse 
arrow  in  the  drawing  area  of  the  tablet.  A  red  box  will  appear  and  move  about 
the  window  as  the  pointer  is  moved.  Place  the  window  in  a  selected  position  and 
click  the  left  mouse  button  to  drop  the  box.  A  3D  room  will  appear  on  the  page. 
This  is  the  drawing  surface  for  the  3D  object,  figure  A. 5.  Next,  return  to  the  “3D 
Gizmo”  and  select  any  of  the  Simple  models  using  the  “point  and  click”  method. 
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Figure  A. 5:  Showcase  3D  Drawing  Window 


Tubular  and  beveled  surfaces  can  be  chosen  also.  The  Faceted  and  Smooth  sliding 
scale  tool  adjusts  the  number  of  surfaces  the  object  will  have.  The  smoother  the 
surface,  the  more  memory  the  object  will  require.  Moving  the  mouse  arrow  back  to 
the  3D  drawing  window,  place  the  pointer  in  the  appropriate  spot  and  hold  the  left 
mouse  button  down  while  dragging  the  object  outward,  figure  A. 6.  In  the  case  of  a 
sphere,  the  size  of  the  radius  depends  on  how  far  the  pointer  has  been  dragged  from 
the  initial  position.  Selecting  the  Material  and  Texture  of  the  object  produced  is  self 
explanatory  and  well  worth  the  time  spent  experimenting.  Once  the  3-D  object  has 
been  produced,  the  orientation  of  the  object  within  the  3-D  drawing  window  can  be 
manipulated  using  either  the  “hand”  tool  on  the  “3D  Gizmo”  window  or  by  placing 
the  mouse  arrow  on  any  surface  “wall”  inside  the  3-D  drawing  window  and  holding 
left  mouse  button  down  while  moving  the  mouse.  The  room  will  rotate  to  the  desired 


Figure  A. 6:  Showcase  3D  Sphere 

orientation. 

Once  the  3-D  model  has  been  produced,  place  the  mouse  arrow  outside  of 
the  3-D  drawing  window  and  click  the  left  mouse  button.  The  drawing  window  will 
disappear  and  1-D  objects  can  once  again  be  produced  on  or  around  the  3-D  object, 
figure  A. 7. 

2.  Saving  the  Drawing 

The  object  can  be  saved  in  a  variety  of  formats.  The  default  style  is  a 
“Showcase”  document.  All  files  should  be  saved  in  this  format  so  that  they  can 
edited  at  a  later  date.  Encapsulated  Postscript  (EPS)  and  Postscript  (PS)  styles 
are  the  style  of  choice,  for  they  can  be  imported  into  a  variety  of  word  processing 
programs.  However,  some  programs  do  not  allow  the  operator  to  choose  the  size  of 
the  object  when  it  is  imported  into  the  document.  Since  this  thesis  was  produced 
using  DTgXtypesetting  software,  both  styles  will  be  discussed. 
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Figure  A. 7:  Showcase  3D  Sphere  in  ID  Environment 


3.  Encapsulated  Postscript 

Once  the  object  has  been  created,  return  to  the  pull  down  menus  on  the 
drawing  tablet.  Select  the  “Edit”  pull  down  menu  and  choose  the  “Select  All”  func¬ 
tion.  This  highlights  all  objects  in  the  drawing  window.  Next,  choose  the  “File”  pull 
down  menu  and  select  the  “Save  as”  function.  Moving  the  mouse  right  an  additional 
pull  down  menu  will  allow  the  operator  to  select  the  type  of  file  format  which  the 
object  will  be  saved  as.  Choose  the  “...as  EPS”  function.  A  “Showcase  File  Browser” 
window  will  appear  and  the  file  name  can  either  be  chosen  or  typed  in  at  the  bot¬ 
tom  of  the  window,  figure  A.8.  The  only  problem  with  importing  EPS  files  into  a 
lATgXdocument  is  that  the  size  of  the  object  cannot  be  adjusted  using  “height  and 
width”  commands  where  they  can  be  if  a  PS  file  is  used. 

4.  Postscript 

The  same  procedure  is  used  for  saving  the  object  as  a  PS  file.  However, 
when  saving  a  Postscript  file,  the  default  device  the  file  will  be  saved  to  is  the  printer. 
An  additional  menu  will  appear  which  will  allow  the  operator  to  choose  whether  to 


Figure  A. 8:  Showcase  File  Browser 

print  the  object  to  a  file  or  the  printer,  figure  A. 9. 


&>■  z-M&abfiBm-.  -- 

i 

Figure  A. 9:  Showcase  PS  Save  Window 


5.  Additional  3D  Goodies 

A  slide  show  presentation  function  is  available  by  selecting  the  “Page 
Gizmo”  from  the  “Page”  pull  down  menu  on  the  main  drawing  tablet,  figure  A. 10. 
Experimentation  is  the  best  way  to  learn  the  system! 

B.  SNAPSHOT 

Including  graphics  from  the  display  requires  that  they  be  converted  into  a 
Postscript  file.  To  do  this,  the  SGI  computers  have  the  capability  to  take  a  “snap- 
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Figure  A.  10:  Showcase  Page  Gizmo 


shot”  of  the  graphic  and  save  it  as  a  “*.rgb”  file.  This  file  extension  is  the  default 
to  the  SGI  system.  This  is  accomplished  by  typing  “snapshot”  at  the  winterm  win¬ 
dow.  A  small  window  will  appear  in  the  upper  left  portion  of  the  display,  figure  A.  11. 
Place  the  mouse  arrow  on  the  “Snapshot”  window  and  depress  the  left  mouse  button. 


Wmm 


snapshot 


prowl er:/d4/herringt, 
|%  snapshot 


Figure  A. 11:  SGI  Snapshot  Window 


Drag  the  mouse  over  the  object  that  is  to  be  copied.  A  red  box  will  position  around 
the  object.  The  box  size  can  only  be  controlled  when  the  mouse  arrow  is  on  the 
“Snapshot”  window.  If  the  edge  of  the  box  does  not  move,  the  mouse  arrow  must  be 
re-positioned  on  the  window.  Once  the  object  has  been  boxed,  place  the  mouse  ar¬ 
row  on  the  “Snapshot”  window  and  depress  the  right  mouse  button.  A  small  window 
will  appear  which  will  allow  the  object  to  be  saved  as  a  “*.rgb”  file.  Name  the  file 
accordingly  and  then  “save”  or  “save  and  exit”  the  program. 


C.  XV 


Once  the  object  has  been  saved  as  an  “*.rgb”  file,  it  can  be  converted  to 
Postscript  format  by  using  the  “XV”  program  on  the  SGI  computers.  Type  “xv 
<filename.rgb>”  at  the  in  figure  A. 12. 


Figure  A. 12:  XV  Window 


Place  the  mouse  arrow  on  the  picture  and  click  the  right  mouse  button.  An 
“xv  controls”  window  will  appear,  allowing  the  operator  to  save  the  file,  figure  A.  13. 
Select  the  “Save”  button  on  the  “xv  controls”  window.  A  small  fish  will  swim  in  circles 


Figure  A.13:  XV  Controls  Window 


until  the  “xv  save”  window  appears,  figure  A.  14.  Select  the  “Postscript”  button  on 
the  lower  left  side  of  the  window,  as  well  as  the  “B/W  Dithered”  button,  if  the  file 
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Figure  A.  14:  XV  Save  Window 


is  to  be  saved  in  black  and  white.  Choose  the  file  name  to  save  and  select  the  “Ok” 
button  in  the  upper  right  corner.  The  file  can  now  be  accessed  as  a  Postscript  file 
and  imported  into  any  word  processing  program  that  accepts  that  format. 


D.  SHOWCASE  PRESENTATIONS 

Through  the  use  of  “Showcase”  two  animations  have  been  developed  to  provide 
a  visual  depiction  of  the  coordinate  systems  relationships  and  the  coordinate  transfor¬ 
mation  from  navigation  to  body  frames.  These  are  provided  on  the  following  pages. 
They  can  be  accessed  through  the  “Showcase”  browser  window. 
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Figure  A. 18:  Coordinate  Transformation,  Slide  4 
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Figure  A.23:  Coordinate  Transformation,  Slide  9 
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Figure  A. 28:  Navigation  to  Body  Rotation,  Slide  5 
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Is  this  a  positive 
or  negative  rotation? 
Does  it  matter? 


Yb 

Yn 


Zn  Zb 


Figure  A. 34:  Navigation  to  Body  Rotation,  Slide  11 
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Now  rotate  about 
Yn.  This  aligns 
the  x  axes . 

Is  this  a  positive 
rotation?  Right  Xn 

hand  rule!  Xb 


Zn  Zb 


Figure  A.36:  Navigation  to  Body  Rotation,  Slide  13 
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Now  rotate  about 
the  Xn  axis .  This 
will  align  the  y 
and  z  axes . 


Zn 


Figure  A. 37:  Navigation  to  Body  Rotation,  Slide  14 
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APPENDIX  B:  MATLAB  FUNCTIONS 


*  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  **  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  *  * 
MATLAB  functions  used  in  the  IMU  /  GPS  Integration 

*  +  ***************************************  *************5|C*5iC****5f:***5|<5f:**^ 

function  y=abn2un(vec) 

%  Transform  noisy  accelerations  from  body  fixed  to  universal 

Ax=vec(l) ; 

Ay=vec(2) ; 

Az=vec(3) ; 

euler=vec(4:6) ; 

Tb2u=rb2u(euler) ; 

y=Tb2u*vec (1:3); 

********************************************************************** 
function  Ab=accel (vec) 

%  This  function  calculates  the  acceleration  terms  in  the  body 
%  coordinate  frame 

%  Establish  the  order  of  the  states 
vbo=vec(l :3) ; 
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wb=vec(4:6) ; 

Lambda=vec (7 :9)  ; 

PboU=vec(10 : 12) ; 
dvbo=vec (13:15); 

l  Calculate  the  cross  product  of  wb  and  vbo 
xprod=crpr(wb,vbo) ; 

%  Transform  Lambda  into  the  body  fixed  coordinate  frame 
BLambda=ru2b (Lambda) ; 

Y,  Multiply  BLambda  by  the  gravity  term  gu 
gb=BLambda*gravitya(PboU) ; 

%  Calculate  dvbo  +  (wb  X  vbo)  -  Ru2b*gu 
Ab=dvbo+xprod-gb ; 

function  y  =  crpr( omega, x) 

V,  This  subroutine  computes  the  crossproduct  of  omega  and  x 
7.  y  =  omega  X  x 
p  =  omega (1) ; 
q  =  omega (2) ; 
r  =  omega (3) ; 

t  =  [0  -r  q;  r  0  -p;-q  p  0] ; 
y  =  t  *  x; 
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******************************************************* *************** 


function  EDMstate=eoml (vec) 

%  This  function  will  calculate  the  accelerations  due  to  forces, 
1  moments,  and  control  inputs. 

7.  (1)  Input  aircraft  data  and  initialize  velocity  terms: 

Vi=677;  %  Initial  velocity  (fps) 

W=13000 ;  %  Weight 

S=230;  l  Wing  planform  area 

b=34;  %  Wing  span 

cbar=7 ;  %  Mean  aerodynamic  chord 

alpha. i=. 047 ;  %  Initial  angle  of  attack  (rad)  or  2.7  degrees 

beta_i=0;  %  Initial  sideslip  angle 

rho=. 000588;  %  Density  at  40,000  ft 

g=32.174;  7>  Gravitational  constant 

m=W/g;  %  Mass  of  the  aircraft 

To=1036.75;  %  Initial  thrust 

No=zeros(3, 1) ; 

Ib=[28000  0  -1300;0  18800  0;-1300  0  47000];  %  Inertia  tensor 


Rw2b_i= [cos (alpha_i)*cos(beta_i) , -cos (alpha.i) *sin(beta_i) , sin (alpha.i) ; 
-sin(beta.i) ,cos(beta_i) ,0; 
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sin(alpha_i)*cos(beta_i)  -sin(alpha_i)*sin(beta_i)  cos(alpha_i)] ; 


[vbo] =Rw2b_i* [Vi  00]’; 
uo=vbo(l) ;vo=vbo(2) ;wo=vbo(3) ; 


%  (2)  Input  the  derivative  coefficients  due  to  the  states 


dCx= [ . 104  0  .3000; 

0  -.73  0  0  0  .4; 

.40  0  5.84  0  4.7  0; 

0  -.110  0  -.45  0  .16; 
.05  0  -.64  0  -15.5  0; 

0  .127  0 ‘-.008  0  -.20] ; 

dCxdot=[0  00000; 
000000; 

0  0  2.2  0  0  0; 

000000; 

0  0  -6.7  0  0  0; 

0  0  0  0  0  0]; 

dCdelt= [0  0  0; 

00  .140; 

.46  0  0; 

0  .178  .019; 

-1.24  0  0; 
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0  -.02  -.074] ; 


Cf 1= [ . 0335  0  .41  0  0  0]’; 

%  (3)  Define  state  variables 
vb=vec(l :3) ; 

u=vb(l);  v=vb(2);  w=vb(3) ; 

wb=vec (4:6);  p=wb(l);  q=wb(2);  r=wb(3); 

lambda=vec(7 :9) ;  phi=lambda(l) ;  theta=lambda(2) ;  psi=lambda(3) ; 
cont=vec(l0 : 12) ;  elev=cont (1) ;  ail=cont(2);  rud=cont(3); 
dt=vec(13) ; 

%  (4)  Determine  total  velocity  and  dynamic  pressure 

V=sqrt(u~2  +  v~2  +  w~2) ; 
qbar= .  5*rh.o*V~2; 

7,  (5)  Determine  angle  of  attack  (alpha)  and  sideslip  (beta) 

alpha=asin(w/V) ; 
beta=asin(v/V) ; 

7,  (6)  Develop  diagonal  Sbar  matrix 

Sbar=diag( [-S , S , -S , S*b , S*cbar , S*b] ) ; 
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%  (7)  Develop  Ml  and  M2  matrices 


Ml=diag ( [1/V, 1/V , 1/V ,b/ (2*V) , cbar/ (2*V) ,b/ (2*V) ] ) ; 

M2=diag( [0, .5*b/V~2, .5*cbar/V~23 0,0,0] ) ; 

'/,  (8)  Calculate  mass  inertia  matrix 

MI=[eye(3)/m  zeros(3,3) ;zeros(3,3)  Ib\eye(3)] ; 

*/.  (9)  Develop  wind  to  body  transformation  matrix 

Rw2b= [cos (alpha) *cos (beta)  -cos (alpha) *sin (beta)  - s in ( alpha) ; 
sin(beta)  cos(beta)  0; 

sin (alpha) *cos (beta)  -sin(alpha)*sin(beta)  cos(alpha)]; 

Tw2b=[Rw2b  zeros(3,3);  zeros(3,3)  Rw2b] ; 

%  (10)  Calculate  linear  and  angular  velocity  terms 

SW=-crpr (wb,vb) ; 

ISIW= (lb) \crpr (Ib*wb , wb) ; 

SWM= [SW  ;  ISIW] ; 

Y=MI*Tw2b.*qbar*Sbar*dCx*Ml* [vb-vbo;wb] ; 

V,  (11)  Calculate  universal  to  body  rotation  matrix  (3-2-1) 

Rpsi=[cos(psi)  sin(psi)  0;  -sin(psi)  cos(psi)  0;  001]; 
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Rtheta= [cos (theta) ,0,-sin (theta) ; 0,1,0; sin (theta) ,0,cos(theta)] ; 
Rphi=[l  0  0;  0  cos(phi)  sin(phi);  0  -sin(phi)  cos(phi)]; 

Ru2b=Rphi*Rtheta*Rpsi ; 

Rb2u=Ru2b > ; 

'/o  (12)  Calculate  gravity  and  control  terms 

Fgravu=  [0  0  g] ’ ; 

Fgravb=[Ru2b*Fgravu;  zeros(3,l)]; 

%  Calculate  thrust  force  terms.  Assume  thrust  acts  through  the  eg 
%  and  there  are  no  moments  due  to  thrust 

T= [To/m;0;0]  ; 

Fthrust = [T ; ( lb) \No] *dt ; 

%  (13)  Calculate  aero  forces  and  moments 

F  aeroM=MI*Tw2b . *qbar*Sbar ; 

FaeroC= [Cf l+dCxdot*M2* [(crpr (wb , vb-vbo) ) ; zeros (3 , 1)] +dCdelt*cont] ; 
F  aero=F  aeroM*F  aeroC ; 

°/0  (14)  Calculate  chi 
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chi=[eye(6)  -  MI*Tw2b . *qbar*Sbar*dCxdot*M2] ; 


%  (15)  Calculate  accelerations 

E0M1_6  =  chi\(SWM  +  Y  +  (Fgravb  +  Fthrust  +  Faero)); 

%  (16)  Calculate  Lambdadot 

QLambda=[l  sin (phi) *tan (theta)  cos (phi) *tan(theta) ; 

0  cos (phi)  -sin(phi); 

0  sin (phi) *sec (theta)  cos (phi) *sec (theta) ] ; 

E0M7_9=QLambda*wb;  '/„  Lambdadot 


%  (19)  Calculate  state  output 

E0Mstate= [E0M1_6 ; EDM7_9] ; 

********************************************************************** 
function  xout  =  exact (vec) 

%  This  function  computes  the  pseudo  ranges  by  adding  the  clock 
7.  bias  range  to  the  actual  range.  Input  vector  consists  of  the 
%  actual  aircraft  position  (PboU) ,  the  position  of  each  satellite 
%  (Psi),  euler  angles,  user  equivalent  range  error  (re) 
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Ro=20 . 997e6 ; 

Puoe=[Ro  00]’;  %  Position  of  the  tangent  plane 
%  (Greenwich  meridian) 

%  Assume  tangent  plane  origin  remains  at 
%  the  Greenwich  Meridian. 

Pbou=vec(l:3) ;  %  True  aircraft  positions  from  EOM 

Posl=vec(4:6) ;  %  Satellite  positions  in  earth  fixed  frame 
Pos2=vec(7 : 9) ; 

Pos3=vec(l0 : 12) ; 

Pos4=vec(13 : 15) ; 


%Ru2e-[0  01;%  Transform  tangent  plane  to  earth  fixed 

%0  10; 

XI  o  0]  ; 

Pboe=Pbou  +  Puoe;  %  Transform  aircraft  position  from  body  to 
X  earth  fixed 


rhol=norm(Pboe+Puoe-Posl) ; 
rho2=norm(Pboe+Puoe-Pos2) ; 
rho3=norm(Pboe+Puoe-Pos3) ; 
rho4=norm(Pboe+Puoe-Pos4) ; 
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xout=[rhol  rho2  rho3  rho4]  ' ; 


************* * ****** * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 
function  g  =  gravitya(r) 

l 

*/.  This  function  computes  the  gravity  vector  in  the  tangent  plane 
%  r  is  the  vector  of  the  vehicle's  position  in  tangent  plane 
*/. 

RO  =  20 . 997e6; 
mu  =  32.2*R0~2; 

R  =  (r(l) ~2  +  r(2) ~2  +  (r(3)  +  R0)“2)“.5; 
gx  =  -(mu/R~3)*r(l) ; 
gy  =  -(mu/R~3)*r(2) ; 
gz  =  -(r(3)  +  R0)*mu/R~3; 
g  =  [gx;  gy;  gz] ; 

********************************************************************** 
function  satpos=initsat 

%  Initialize  satellite  and  GPS  receiver  positions 


Rs=285 . 885e6 ;  cl=cos (2*pi/3) ;  sl=sin(2*pi/3) ;  s2=-sl; 
satposl=[0  0  Rs] ’ ;  satpos2=[Rs  0  0] ' ; 

satpos3= [Rs*cl  Rs*sl  0] ’ ;  satpos4=[Rs*cl  Rs*s2  0] ‘ ; 
satpos=[satposl'  satpos2'  satpos3'  satpos4']  ; 
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********************************************************************** 


function  out=pos(vec) 

’/.  This  function  converts  velocity  in  the  body  frame  to  velocity  in 
%  the  inertial  frame 

vb=vec(l:3);  u=vb(l);  v=vb(2);  w=vb(3); 
wb=vec(4:6);  p=wb(l);  q=wb(2) ;  r=wb(3); 

lambda=vec(7 :9) ;  phi=lambda(l) ;  theta=lambda(2) ;  psi=lambda(3) ; 
Rpsi=[cos(psi)  sin(psi)  0;  -sin(psi)  cos(psi)  0;  0  0  1] ; 

Rtheta= [cos (theta)  0  -sin(theta);  010;  sin(theta)  0  cos(theta)]; 
Rphi=[l  0  0;  0  cos(phi)  sin(phi) ;  0  -sin(phi)  cos(phi)]; 

Ru2b=Rphi*Rtheta*Rpsi ; 

Rb2u=Ru2b  * ; 

out=Rb2u*vb ; 

**************  ****************************************************  **  ** 
function  xout  =  pseudo (vec) 

%  This  function  computes  the  pseudo  ranges  by  adding  the  clock 
%  bias  range  to  the  actual  range.  Input  vector  consists  of  the 
%  actual  aircraft  position  (PboU) ,  the  position  of  each  satellite 
%  (Psi) ,  euler  angles  user  equivalent  range  error  (re) 
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Ro=6 . 38e6 ; 

Puoe=[Ro  0  0] ' ;  %  Position  of  the  tangent  plane. 

1  Assume  tangent  plane  origin  remains  at 
%  the  Greenwich  Meridian. 

Pbou=vec(l : 3) ;  %  True  aircraft  positions  from  EOM 

Posl=vec(4:6) ;  %  Satellite  positions  in  earth  fixed  frame 
Pos2=vec(7 : 9) ; 

Pos3=vec(l0: 12) ; 

Pos4=vec(13: 15) ; 

re=vec(16);  '/,  User  equivalent  range  error 

Ru2e=[0  0  1;  %  Transform  tangent  plane  to  earth  fixed 
0  10; 

10  0]; 

Pboe=Ru2e*Pbou  +  Puoe;  %  Transform  aircraft  position  from  body  to 
%  earth  fixed 


rhol=norm(Pboe+Puoe-Posl)+  re; 
rho2=norm(Pboe+Puoe-Pos2)+  re; 
rho3=norm(Pboe+Puoe-Pos3)+  re; 
rho4=norm(Pboe+Puoe-Pos4)+  re; 
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xout=[rhol  rho2  rho3  rho4] ' ; 


***********  *  **  3*C?*C  *  3*C3#C  ******  ***  *  *  *  *  *  *  **  **  *  **  *  *  *  *  *  *  **  *  **  *  5f:  *  **  *  **  *  **  *  **  **  %  * 

function  y=rb2u(x) 

'/.  Calculate  inertial  to  body  rotation  matrix  (3-2-1) 


phi=x(l) ; 
theta=x(2) ; 
psi=x(3) ; 

Rpsi=[cos(psi)  sin(psi)  0;  -sin(psi)  cos(psi)  0;  0  0  1]; 

Rtheta= [cos (theta)  0  -sin (theta) ;  010;  sin(theta)  0  cos(theta)]; 
Rphi=[l  0  0;  0  cos(phi)  sin(phi);  0  -sin(phi)  cos(phi)]; 

y=[Rphi*Rtheta*Rpsi] 1 ; 

******************** ****************************************** ** ****** 
function  y=ru2b(x) 

%  Calculate  body  to  inertial  rotation  matrix 

phi  =  x(l);  theta  =  x(2) ;  psi  =  x(3) ; 

Rpsi=[cos(psi)  sin(psi)  0;  -sin(psi)  cos(psi)  0;  00  1]; 
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Rtheta= [cos (theta)  0  -sin(theta)  ;  0  10;  sin(theta)  0  cos(theta)]; 
Rphi=[l  0  0;  0  cos(phi)  sin(phi) ;  0  -sin(phi)  cos(phi)]; 

y =Rph i *Rt het  a*Rp  s i ; 

******  ************************************  **********  *  ***************** 
7o  Function  kal.m 

°/0  This  function  performs  the  linearization  of  the  Kalman  filter. 

°/0  First  I  will  linearize  the  navigation  model  "navmod.m".  The  inputs 
*/.  are  Ax,  Ay,  and  Az  from  the  IMU  and  drift  error  for  the  exact  range 
%  calculations.  Outputs  are  the  four  pseudo  ranges. 

[a,b,c,d]  =linmod(  ^avraod' ) 

pause 

%  Next,  I  will  include  these  matrices  in  the  synthesis  model, 

%  "syn.m".  Inputs  1  through  4  will  be  noisy  accelerations  where 
%  I  will  append  transmissions  zeros  at  -1  and  include  an  integrator. 

°/0  Input  4  will  be  noise  from  the  clock  model.  Inputs  5  through  7 
’/.  will  be  the  accelerations  from  the  IMU. 

[as ,bs , cs , ds] =linmod( ' syn ’ ) 

pause 
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%  Next  I  will  determine  the  Kalman  gains  from  the  noise  inputs,  1-4. 
X  I  chose  Q=1  and  R=1  initially. 

Q=eye(4) ;R=eye(4) ; 

l=lqe(as ,bs( : ,1:4) ,cs,Q,R) 

l  I  will  check  the  bode  response  of  the  pseudo  range  inputs  to  the 
1  pseudo  range  estimates.  I  will  use  a  function  called  "bplots.m". 

bplots 

l  I  will  adjust  the  weighting  values  to  obtain  a  bandwidth  of 
X  1  rad/ sec. 

%  Decrease  R  to  5 . 

l=lqe (as , bs ( : , 1 : 4) , cs , Q , 5*R) 
bplots 


l=lqe (as , bs ( : , 1 : 4) , cs , . 5*Q , 5*R) 
bplots 


l=lqe(as,bs( : , 1 :4) ,cs, . 05*Q ,50*R) 
bplots 
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l=lqe(as ,bs( : , 1 :4) ,cs , . 05*Q, 6*R) 
bplots 

%  I  will  use  the  values  of  Q=.05  and  R=6.  These  provide  bandwidths 
l  of  1  rad/sec. 

%  Next,  I  will  look  at  the  response  of  the  filter  from  the 
X  accelerations  (input)  to  the  pseudo  ranges  (output) .  I  will  use 
%  a  function  called  "accplot.m". 

accplot 

********************  **  *  *  *  ***  **  ***  ***  ***  ***  5*C3*C  **  *  **  *  **  *  **  **  ***  *  **  *  **  **  *  * 

X  Function  bplots. m 

%  This  function  plots  each  input  bode  response 

w=logspace(-4,2, 100) ; 
x=-3*ones (size (w) ) ; 

[mag,phase]=bode(as-l*cs,l,cs,ds( : ,1 :4) ,l,w) ; 
magdb=20*log(mag) ; 
subplot (211) 

semilogx(w,magdb,w,x, ’ . }) 
grid 

title (’ Bode  Response  for  Pseudo  ranges  1  &  2') 
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axis([le-4  le2  -10  10]) 

pause 

°/„  print 

[mag, phase] =bode(as-l*cs,l,cs,ds(: ,1:4) ,2, w) ; 
magdb=20*log(mag) ; 
subplot (212) 

semilogx(w,magdb,w,x, ’ . ’) 
grid 

axis([le-4  le2  -10  10]) 
pause 
%  print 

[mag, phase] =bode (as-l*cs, 1, cs, ds ( : ,1:4) ,3,w) ; 
magdb=20*log(mag)  ; 
subplot (211) 

semilogx(w,magdb,w,x, ‘ . ’) 
grid 

title ('Bode  Response  for  Pseudo  ranges  3  &  4') 
axis([le-4  le2  -10  10]) 
pause 
%  print 

[mag, phase] =bode (as-l*cs, 1, cs, ds ( : ,1:4) ,4,w) ; 
magdb=20*log(mag) ; 
subplot (212) 
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semilogx(w,magdb?w,x, ; 
grid 

axis( [le-4  le2  -10  10]) 

pause 

°/0  print 

*  sf:  sfc  *  5{C  5fc3jc  *  **  s|c  *  sfc  *  s|c  *  *  *  *  +  5|c  +  **  *  **  sfc  sf:  s|e  sfc *  >jc  5|c sfc  *  *:+::+:*  *  **  sjc  jfcjjc  sjc  **  jf: :+:  if:*  3f=  **  *  He*  **  *  * 

%  Function  accplot.m 

%  This  function  plots  each  input  bode  response  for  the  accelerations 

w=logspace(-4,2, 100) ; 
x=-3*ones(size(w) ) ; 

[mag, phase] =bode(as-l*cs,bs( : ,5:7) ,cs,ds( : ,5:7) ,l,w) ; 
magdb=20*log(mag) ; 
subplot (311) 

semilogx(w,magdb,w,x, ' . ’ ) 
grid 

titleC'Bode  Response  for  Ax,  Ay,  Az') 
axis([le-4  le2  -100  100]) 
pause 
%  print 

[mag, phase] =bode(as-l*cs,bs( : ,5 :7) ,cs,ds( : ,5 :7) ,2,w) ; 
magdb=20*log(mag) ; 
subplot (312) 
semilogx(w,magdb,w,x, 1 . 
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grid 


axis( [le-4  le2  -100  100]) 
pause 
X  print 

[mag, phase] =bode(as-l*cs,bs( : ,5 :7) ,cs,ds( : ,5 :7) ,3,w) ; 

magdb=20*log(mag) ; 

subplot (313) 

semilogx(w,magdb, w,x, ’ . 

grid 

axis([le-4  le2  -100  100]) 
pause 
l  print 

********************************************************************** 
%  Function  plotstep.m 

%  This  function  linearizes  the  Kalman  filter  with  gains  and  produces  a 
1  step  response  due  to  accelerations  in  the  x,y,  and  z  axes. 

[af , bf , cf , df ] =linmod ( ’ kf ilnav2 * ) ; 

[Y,X,t]=step(af ,bf ,cf ,df ) ; 

plot (t ,Y) 
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title(JStep  response  due  to  inputs  in  Ax,Ay,Az;) 
pause 
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